diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index f55c668ec0003a69494557365946bd825cdc057d..8a7f2ccd88a0e90ca22e74ec6156a324c602cb93 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -66,7 +66,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpBatch instead");
+ assert(_ftr_ptr->getMeasurement().size() == 3 && "FactorGnssTdcp3d uses 3d measurements. For FeatureGnssTdcp with also delta clock, use FactorGnssTdcpWithClock instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_with_clock.h
similarity index 86%
rename from include/gnss/factor/factor_gnss_tdcp_batch.h
rename to include/gnss/factor/factor_gnss_tdcp_with_clock.h
index 1c0e9ecff7deccc491182c04e83c3044411e6c98..e0e84a459cf4119b0471658608e9a99eeb577f5a 100644
--- a/include/gnss/factor/factor_gnss_tdcp_batch.h
+++ b/include/gnss/factor/factor_gnss_tdcp_with_clock.h
@@ -20,8 +20,7 @@
//
//--------LICENSE_END--------
-#ifndef FACTOR_GNSS_TDCP_BATCH_H_
-#define FACTOR_GNSS_TDCP_BATCH_H_
+#pragma once
//Wolf includes
#include "core/common/wolf.h"
@@ -30,22 +29,22 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FactorGnssTdcpBatch);
+WOLF_PTR_TYPEDEFS(FactorGnssTdcpWithClock);
-class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
+class FactorGnssTdcpWithClock : public FactorAutodiff<FactorGnssTdcpWithClock, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
{
protected:
SensorGnssPtr sensor_gnss_ptr_;
public:
- FactorGnssTdcpBatch(const FeatureBasePtr& _ftr_ptr,
+ FactorGnssTdcpWithClock(const FeatureBasePtr& _ftr_ptr,
const CaptureBasePtr& _capture_other_ptr,
const SensorGnssPtr& _sensor_gnss_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function = false,
FactorStatus _status = FAC_ACTIVE) :
- FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpBatch",
+ FactorAutodiff<FactorGnssTdcpWithClock, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>("FactorGnssTdcpWithClock",
TOP_GEOM,
_ftr_ptr,
_capture_other_ptr->getFrame(),
@@ -67,11 +66,11 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
_sensor_gnss_ptr->getEnuMapYaw()),
sensor_gnss_ptr_(_sensor_gnss_ptr)
{
- assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpBatch uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
+ assert(_ftr_ptr->getMeasurement().size() == 4 && "FactorGnssTdcpWithClock uses 4d measurements (pos.displacement, delta clock). For FeatureGnssTdcp with only displacement, use FactorGnssTdcp3d instead");
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined(), "Creating a GNSS SingleDiff 3D factor without initializing ENU");
}
- ~FactorGnssTdcpBatch() override = default;
+ ~FactorGnssTdcpWithClock() override = default;
template<typename T>
bool operator ()(const T* const _x1,
@@ -89,7 +88,7 @@ class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4,
};
template<typename T>
-inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
+inline bool FactorGnssTdcpWithClock::operator ()(const T* const _x1,
const T* const _o1,
const T* const _t1,
const T* const _x2,
@@ -109,20 +108,20 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
Eigen::Map<Eigen::Matrix<T,3,1> > disp_residuals(_residuals);
Eigen::Map<Eigen::Matrix<T,4,1> > residuals(_residuals);
- Eigen::Matrix<T,3,3> R_ECEF_ENU = sensor_gnss_ptr_->getREnuEcef().transpose().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::Quaternion<T> q_ECEF_ENU = sensor_gnss_ptr_->getQEnuEcef().conjugate().cast<T>();
+ Eigen::Quaternion<T> q_ENU_MAP = sensor_gnss_ptr_->computeQEnuMap(_roll_ENU_MAP[0], _pitch_ENU_MAP[0], _yaw_ENU_MAP[0]);
// Expected d
Eigen::Matrix<T,4,1> exp;
// Expected displacement in ECEF
- exp.head(3) = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+ exp.head(3) = q_ECEF_ENU * q_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
// clock error
exp(3) = *_t2 - *_t1;
// Compute residual
- residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (exp - getMeasurement().cast<T>());
+ residuals = getMeasurementSquareRootInformationUpper() * (exp - getMeasurement());
//std::cout << "frame1: " << _x1[0] << " " << _x1[1] << " " << _x1[2] << " " << _o1[0] << " " << _o1[1] << " " << _o1[2] << " " << _o1[3] << std::endl;
//std::cout << "frame2: " << _x2[0] << " " << _x2[1] << " " << _x2[2] << " " << _o2[0] << " " << _o2[1] << " " << _o2[2] << " " << _o2[3] << std::endl;
@@ -134,6 +133,4 @@ inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
return true;
}
-} // namespace wolf
-
-#endif
+} // namespace wolf
\ No newline at end of file
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 802a7ca5f1c385da8335b9512121a0ebce5f154b..3ac5d6817e29641352a7e7b8ca5d8bafd79838a1 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -25,7 +25,7 @@
#include "gnss/feature/feature_gnss_tdcp.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "gnss/factor/factor_gnss_tdcp.h"
-#include "gnss/factor/factor_gnss_tdcp_batch.h"
+#include "gnss/factor/factor_gnss_tdcp_with_clock.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include "gnss/factor/factor_gnss_fix_3d.h"
#include "gnss_utils/utils/rcv_position.h"
@@ -493,7 +493,7 @@ void ProcessorTrackerGnss::establishFactors()
tdcp_output.cov_d);
// EMPLACE FACTOR
- last_fac_ptr = FactorBase::emplace<FactorGnssTdcpBatch>(ftr,
+ last_fac_ptr = FactorBase::emplace<FactorGnssTdcpWithClock>(ftr,
ftr,
ref_cap_gnss,
sensor_gnss_,
diff --git a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
index 6d97ad40a884683c13672c0f7b68069abfe17873..904eb459b270e8b8bb57549114e9f6f4afa2085a 100644
--- a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
@@ -50,7 +50,7 @@ void TreeManagerSlidingWindowTdcp::keyFrameCallback(FrameBasePtr _key_frame)
(fac->getType() == "FactorGnssTdcp" or
fac->getType() == "FactorGnssTdcp2d" or
fac->getType() == "FactorGnssTdcp3d" or
- fac->getType() == "FactorGnssTdcpBatch"))
+ fac->getType() == "FactorGnssTdcpWithClock"))
fac->setStatus(FAC_ACTIVE);
}
}