diff --git a/include/gnss/capture/capture_gnss.h b/include/gnss/capture/capture_gnss.h
index 9bad62dd81e356d0d415bcb86e3ff89f7122d2e5..d69cf4a321ed4e11cd7080b11f850635942a3e6d 100644
--- a/include/gnss/capture/capture_gnss.h
+++ b/include/gnss/capture/capture_gnss.h
@@ -20,77 +20,76 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "core/capture/capture_base.h"
-//std includes
+// std includes
#include "gnss_utils/snapshot.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureGnss);
-//class CaptureGnss
+// class CaptureGnss
class CaptureGnss : public CaptureBase
{
- protected:
- GnssUtils::SnapshotPtr snapshot_; ///< observation and navigation data
-
- public:
- CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot);
- ~CaptureGnss() override;
-
- GnssUtils::SnapshotConstPtr getSnapshot() const;
- GnssUtils::SnapshotPtr getSnapshot();
- GnssUtils::ObservationsConstPtr getObservations() const;
- GnssUtils::ObservationsPtr getObservations();
- GnssUtils::NavigationConstPtr getNavigation() const;
- GnssUtils::NavigationPtr getNavigation();
- const GnssUtils::Satellites& getSatellites() const;
- GnssUtils::Satellites& getSatellites();
-
+ protected:
+ GnssUtils::SnapshotPtr snapshot_; ///< observation and navigation data
+
+ public:
+ CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot);
+ ~CaptureGnss() override;
+
+ GnssUtils::SnapshotConstPtr getSnapshot() const;
+ GnssUtils::SnapshotPtr getSnapshot();
+ GnssUtils::ObservationsConstPtr getObservations() const;
+ GnssUtils::ObservationsPtr getObservations();
+ GnssUtils::NavigationConstPtr getNavigation() const;
+ GnssUtils::NavigationPtr getNavigation();
+ const GnssUtils::Satellites& getSatellites() const;
+ GnssUtils::Satellites& getSatellites();
};
inline GnssUtils::SnapshotConstPtr CaptureGnss::getSnapshot() const
{
- return snapshot_;
+ return snapshot_;
}
inline GnssUtils::SnapshotPtr CaptureGnss::getSnapshot()
{
- return snapshot_;
+ return snapshot_;
}
inline GnssUtils::ObservationsConstPtr CaptureGnss::getObservations() const
{
- return snapshot_->getObservations();
+ return snapshot_->getObservations();
}
inline GnssUtils::ObservationsPtr CaptureGnss::getObservations()
{
- return snapshot_->getObservations();
+ return snapshot_->getObservations();
}
inline GnssUtils::NavigationConstPtr CaptureGnss::getNavigation() const
{
- return snapshot_->getNavigation();
+ return snapshot_->getNavigation();
}
inline GnssUtils::NavigationPtr CaptureGnss::getNavigation()
{
- return snapshot_->getNavigation();
+ return snapshot_->getNavigation();
}
inline GnssUtils::Satellites& CaptureGnss::getSatellites()
{
- return snapshot_->getSatellites();
+ return snapshot_->getSatellites();
}
inline const GnssUtils::Satellites& CaptureGnss::getSatellites() const
{
- return snapshot_->getSatellites();
+ return snapshot_->getSatellites();
}
-} //namespace wolf
+} // namespace wolf
diff --git a/include/gnss/capture/capture_gnss_fix.h b/include/gnss/capture/capture_gnss_fix.h
index b7e4fb27f1ed6e4d7b6b0432096dd526dc82c255..50764f12967cd3713428e3d18a0908e0232143f2 100644
--- a/include/gnss/capture/capture_gnss_fix.h
+++ b/include/gnss/capture/capture_gnss_fix.h
@@ -20,48 +20,48 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "gnss/feature/feature_gnss_fix.h"
#include "core/capture/capture_base.h"
-//std includes
+// std includes
//
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureGnssFix);
-//class CaptureGnssFix
+// class CaptureGnssFix
class CaptureGnssFix : public CaptureBase
{
- protected:
- Eigen::Vector4d fix_ECEF_; ///< position in ECEF coordinates and clock_bias.
- Eigen::Matrix4d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
- TimeStamp ts_GPST_; ///< Time stamp in GPS time
-
- public:
- CaptureGnssFix(const TimeStamp&,
- SensorBasePtr,
- const Eigen::Vector4d&,
- const Eigen::Matrix4d&,
- bool _ecef_coordinates = true);
- ~CaptureGnssFix() override;
-
- Eigen::Vector4d getFixEcef() const;
- Eigen::Matrix4d getFixCovarianceEcef() const;
- double getClockBias() const;
- double getClockBiasVariance() const;
- Eigen::Vector3d getPositionEcef() const;
- Eigen::Matrix3d getPositionCovarianceEcef() const;
- void getPositionAndCovarianceEcef(Eigen::Vector3d&, Eigen::Matrix3d&) const;
- TimeStamp getGpsTimeStamp() const;
-
- void setFixEcef(const Eigen::Vector4d&);
- void setFixCovarianceEcef(const Eigen::Matrix4d&);
- void setClockBias(const double&);
- void setPositionEcef(const Eigen::Vector3d&);
- void setGpsTimeStamp(const TimeStamp&);
+ protected:
+ Eigen::Vector4d fix_ECEF_; ///< position in ECEF coordinates and clock_bias.
+ Eigen::Matrix4d covariance_ECEF_; ///< Noise of the fix in ECEF coordinates.
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
+
+ public:
+ CaptureGnssFix(const TimeStamp&,
+ SensorBasePtr,
+ const Eigen::Vector4d&,
+ const Eigen::Matrix4d&,
+ bool _ecef_coordinates = true);
+ ~CaptureGnssFix() override;
+
+ Eigen::Vector4d getFixEcef() const;
+ Eigen::Matrix4d getFixCovarianceEcef() const;
+ double getClockBias() const;
+ double getClockBiasVariance() const;
+ Eigen::Vector3d getPositionEcef() const;
+ Eigen::Matrix3d getPositionCovarianceEcef() const;
+ void getPositionAndCovarianceEcef(Eigen::Vector3d&, Eigen::Matrix3d&) const;
+ TimeStamp getGpsTimeStamp() const;
+
+ void setFixEcef(const Eigen::Vector4d&);
+ void setFixCovarianceEcef(const Eigen::Matrix4d&);
+ void setClockBias(const double&);
+ void setPositionEcef(const Eigen::Vector3d&);
+ void setGpsTimeStamp(const TimeStamp&);
};
inline Eigen::Vector4d CaptureGnssFix::getFixEcef() const
@@ -81,7 +81,7 @@ inline double CaptureGnssFix::getClockBias() const
inline double CaptureGnssFix::getClockBiasVariance() const
{
- return covariance_ECEF_(3,3);
+ return covariance_ECEF_(3, 3);
}
inline Eigen::Vector3d CaptureGnssFix::getPositionEcef() const
@@ -91,13 +91,13 @@ inline Eigen::Vector3d CaptureGnssFix::getPositionEcef() const
inline Eigen::Matrix3d CaptureGnssFix::getPositionCovarianceEcef() const
{
- return covariance_ECEF_.topLeftCorner<3,3>();
+ return covariance_ECEF_.topLeftCorner<3, 3>();
}
inline void CaptureGnssFix::getPositionAndCovarianceEcef(Eigen::Vector3d& position, Eigen::Matrix3d& cov) const
{
position = fix_ECEF_.head<3>();
- cov = covariance_ECEF_.topLeftCorner<3,3>();
+ cov = covariance_ECEF_.topLeftCorner<3, 3>();
}
inline wolf::TimeStamp CaptureGnssFix::getGpsTimeStamp() const
@@ -124,10 +124,10 @@ inline void CaptureGnssFix::setPositionEcef(const Eigen::Vector3d& _fix_position
{
fix_ECEF_.head<3>() = _fix_position;
}
-
+
inline void CaptureGnssFix::setGpsTimeStamp(const TimeStamp& _ts_GPST)
{
ts_GPST_ = _ts_GPST;
}
-} //namespace wolf
+} // namespace wolf
diff --git a/include/gnss/capture/capture_gnss_tdcp.h b/include/gnss/capture/capture_gnss_tdcp.h
index 47615c2ef877c3eb0cef68031340a4eff82a6654..8da3c94dcc516fbaa6cf98caad8252ae2e3a7b76 100644
--- a/include/gnss/capture/capture_gnss_tdcp.h
+++ b/include/gnss/capture/capture_gnss_tdcp.h
@@ -20,45 +20,44 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "core/capture/capture_base.h"
#include "gnss/feature/feature_gnss_displacement.h"
-//std includes
+// std includes
//
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(CaptureGnssTdcp);
class CaptureGnssTdcp : public CaptureBase
{
- protected:
- Eigen::Vector3d data_; ///< Displacement in ECEF coordinates.
- Eigen::Matrix3d data_covariance_; ///< Noise of the capture.
- FrameBasePtr origin_frame_ptr_;
- TimeStamp ts_GPST_; ///< Time stamp in GPS time
+ protected:
+ Eigen::Vector3d data_; ///< Displacement in ECEF coordinates.
+ Eigen::Matrix3d data_covariance_; ///< Noise of the capture.
+ FrameBasePtr origin_frame_ptr_;
+ TimeStamp ts_GPST_; ///< Time stamp in GPS time
- public:
- CaptureGnssTdcp(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector3d& _data,
- const Eigen::Matrix3d& _data_covariance,
- const FrameBasePtr& _origin_frame_ptr) :
- CaptureBase("CaptureGnssTdcp", _ts, _sensor_ptr),
- data_(_data),
- data_covariance_(_data_covariance),
- origin_frame_ptr_(_origin_frame_ptr)
- {};
- ~CaptureGnssTdcp() override{};
- const Eigen::Vector3d& getData() const;
- const Eigen::Matrix3d& getDataCovariance() const;
- void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
- FrameBaseConstPtr getOriginFrame() const;
- FrameBasePtr getOriginFrame();
- const TimeStamp& getGpsTimeStamp() const;
- void setGpsTimeStamp(const TimeStamp &_ts_GPST);
+ public:
+ CaptureGnssTdcp(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector3d& _data,
+ const Eigen::Matrix3d& _data_covariance,
+ const FrameBasePtr& _origin_frame_ptr)
+ : CaptureBase("CaptureGnssTdcp", _ts, _sensor_ptr),
+ data_(_data),
+ data_covariance_(_data_covariance),
+ origin_frame_ptr_(_origin_frame_ptr){};
+ ~CaptureGnssTdcp() override{};
+ const Eigen::Vector3d& getData() const;
+ const Eigen::Matrix3d& getDataCovariance() const;
+ void getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const;
+ FrameBaseConstPtr getOriginFrame() const;
+ FrameBasePtr getOriginFrame();
+ const TimeStamp& getGpsTimeStamp() const;
+ void setGpsTimeStamp(const TimeStamp& _ts_GPST);
};
inline const Eigen::Vector3d& CaptureGnssTdcp::getData() const
@@ -73,7 +72,7 @@ inline const Eigen::Matrix3d& CaptureGnssTdcp::getDataCovariance() const
inline void CaptureGnssTdcp::getDataAndCovariance(Eigen::Vector3d& data, Eigen::Matrix3d& data_cov) const
{
- data = data_;
+ data = data_;
data_cov = data_covariance_;
}
@@ -92,9 +91,9 @@ inline const TimeStamp& CaptureGnssTdcp::getGpsTimeStamp() const
return ts_GPST_;
}
-inline void CaptureGnssTdcp::setGpsTimeStamp(const TimeStamp &_ts_GPST)
+inline void CaptureGnssTdcp::setGpsTimeStamp(const TimeStamp& _ts_GPST)
{
ts_GPST_ = _ts_GPST;
}
-} //namespace wolf
+} // namespace wolf
diff --git a/include/gnss/common/gnss.h b/include/gnss/common/gnss.h
index 376ab1880794a45e49e8711a0fdc0825f448fbd7..d17b74cf704924ae40f2710fe85c717f7fc1ee55 100644
--- a/include/gnss/common/gnss.h
+++ b/include/gnss/common/gnss.h
@@ -27,8 +27,7 @@
namespace wolf
{
-
// Folder schema Registry
WOLF_REGISTER_FOLDER("gnss", _WOLF_GNSS_SCHEMA_DIR);
-}
+} // namespace wolf
diff --git a/include/gnss/factor/factor_gnss_displacement_2d.h b/include/gnss/factor/factor_gnss_displacement_2d.h
index 8ee8980680c19e76bd821d1bd979e3dbea1633eb..715333c7215f582562c85cc564f355bde0685c5e 100644
--- a/include/gnss/factor/factor_gnss_displacement_2d.h
+++ b/include/gnss/factor/factor_gnss_displacement_2d.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssDisplacement2d);
class FactorGnssDisplacement2d : public FactorAutodiff<FactorGnssDisplacement2d, 3, 2, 1, 2, 1, 3, 1, 1, 1>
diff --git a/include/gnss/factor/factor_gnss_displacement_3d.h b/include/gnss/factor/factor_gnss_displacement_3d.h
index 45984ce5f44a037d3bac0f349779d52e34b7a3ce..0dac2adc5d9dd89a22ddeac58f954656c9a90dac 100644
--- a/include/gnss/factor/factor_gnss_displacement_3d.h
+++ b/include/gnss/factor/factor_gnss_displacement_3d.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssDisplacement3d);
class FactorGnssDisplacement3d : public FactorAutodiff<FactorGnssDisplacement3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>
@@ -38,11 +37,11 @@ class FactorGnssDisplacement3d : public FactorAutodiff<FactorGnssDisplacement3d,
public:
FactorGnssDisplacement3d(const 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)
+ const FrameBasePtr& _frame_other_ptr,
+ const SensorGnssPtr& _sensor_gnss_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE)
: FactorAutodiff<FactorGnssDisplacement3d, 3, 3, 4, 3, 4, 3, 1, 1, 1>(
"FactorGnssDisplacement3d",
TOP_GEOM,
@@ -85,14 +84,14 @@ class FactorGnssDisplacement3d : public FactorAutodiff<FactorGnssDisplacement3d,
template <typename T>
inline bool FactorGnssDisplacement3d::operator()(const T* const _x1,
- const T* const _o1,
- const T* const _x2,
- const T* const _o2,
- const T* const _x_antena,
- const T* const _roll_ENU_MAP,
- const T* const _pitch_ENU_MAP,
- const T* const _yaw_ENU_MAP,
- T* _residuals) const
+ const T* const _o1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _x_antena,
+ const T* const _roll_ENU_MAP,
+ const T* const _pitch_ENU_MAP,
+ const T* const _yaw_ENU_MAP,
+ T* _residuals) const
{
Eigen::Map<const Eigen::Matrix<T, 3, 1>> t_MAP_BASE1(_x1);
Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE1(_o1);
diff --git a/include/gnss/factor/factor_gnss_displacement_3d_with_clock.h b/include/gnss/factor/factor_gnss_displacement_3d_with_clock.h
index a27ce38103f7e69d8dc2ef11375970a4d8313d95..ef3107666fb1b513bf384e3c155b01cd0dbcf0ef 100644
--- a/include/gnss/factor/factor_gnss_displacement_3d_with_clock.h
+++ b/include/gnss/factor/factor_gnss_displacement_3d_with_clock.h
@@ -27,7 +27,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssDisplacement3dWithClock);
class FactorGnssDisplacement3dWithClock
diff --git a/include/gnss/factor/factor_gnss_fix_2d.h b/include/gnss/factor/factor_gnss_fix_2d.h
index 6b9197233fa5c700344e84faef8958d8627add0d..47ba81d1f4102d43c0688df53fcec8c02ef0fd37 100644
--- a/include/gnss/factor/factor_gnss_fix_2d.h
+++ b/include/gnss/factor/factor_gnss_fix_2d.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssFix2d);
class FactorGnssFix2d : public FactorAutodiff<FactorGnssFix2d, 3, 2, 1, 3, 3, 1, 1, 1>
diff --git a/include/gnss/factor/factor_gnss_fix_3d.h b/include/gnss/factor/factor_gnss_fix_3d.h
index 58c53d8b2de507ca594975d14f9a2be8a48b8e73..bf817468936c71ecee7c6b865a2f9d01a8318054 100644
--- a/include/gnss/factor/factor_gnss_fix_3d.h
+++ b/include/gnss/factor/factor_gnss_fix_3d.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssFix3d);
class FactorGnssFix3d : public FactorAutodiff<FactorGnssFix3d, 3, 3, 4, 3, 3, 1, 1, 1>
diff --git a/include/gnss/factor/factor_gnss_fix_3d_with_clock.h b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
index 352e7a03bdba4265f95fe0f6c7a6b78b94580726..4c715a466dbee43a7dfc7e6ea40466051f3f806f 100644
--- a/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
+++ b/include/gnss/factor/factor_gnss_fix_3d_with_clock.h
@@ -28,7 +28,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssFix3dWithClock);
class FactorGnssFix3dWithClock : public FactorAutodiff<FactorGnssFix3dWithClock, 4, 3, 4, 3, 3, 1, 1, 1, 1>
diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index 869cebb43fd29ba30d9257c3b9bc6affe681cced..42d565af6d7711ba03ff58607b225986d7cf65ff 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -30,7 +30,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssPseudoRange);
class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3, 4, 1, 1, 3, 3, 1, 1, 1>
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index eb2708c3a6318a4ee4df03bb161231bd65c81420..f847f8944888c565f970e1c46fe9f91e171730f6 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -30,7 +30,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(FactorGnssTdcp);
class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1, 3, 3, 1, 1, 1>
diff --git a/include/gnss/feature/feature_gnss_displacement.h b/include/gnss/feature/feature_gnss_displacement.h
index b5d71d5d99915dd580d8a814098ba489a07df4f0..806496213ed765e8eca0163bf3d1d56bd01c6264 100644
--- a/include/gnss/feature/feature_gnss_displacement.h
+++ b/include/gnss/feature/feature_gnss_displacement.h
@@ -20,34 +20,31 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "core/feature/feature_base.h"
-//std includes
-
-namespace wolf {
+// std includes
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureGnssSinleDiff);
-
+
class FeatureGnssDisplacement : public FeatureBase
{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGnssDisplacement(const Eigen::Vector4d& _measurement, const Eigen::Matrix4d& _meas_covariance) :
- FeatureBase("FeatureGnssDisplacement", _measurement, _meas_covariance)
- {};
- FeatureGnssDisplacement(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance) :
- FeatureBase("FeatureGnssDisplacement", _measurement, _meas_covariance)
- {};
-
- ~FeatureGnssDisplacement() override{};
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureGnssDisplacement(const Eigen::Vector4d& _measurement, const Eigen::Matrix4d& _meas_covariance)
+ : FeatureBase("FeatureGnssDisplacement", _measurement, _meas_covariance){};
+ FeatureGnssDisplacement(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance)
+ : FeatureBase("FeatureGnssDisplacement", _measurement, _meas_covariance){};
+
+ ~FeatureGnssDisplacement() override{};
};
-} // namespace wolf
+} // namespace wolf
diff --git a/include/gnss/feature/feature_gnss_fix.h b/include/gnss/feature/feature_gnss_fix.h
index 972207413b77b0259d5a2460c35cc81eb21c1f71..19d191e361ae01e92c81d3057f1176197dbd9c99 100644
--- a/include/gnss/feature/feature_gnss_fix.h
+++ b/include/gnss/feature/feature_gnss_fix.h
@@ -20,45 +20,42 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "core/feature/feature_base.h"
-//std includes
+// std includes
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/utils/rcv_position.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureGnssFix);
-//class FeatureGnssFix
+// class FeatureGnssFix
class FeatureGnssFix : public FeatureBase
{
- public:
-
- /** \brief Constructor from capture pointer and measure
- *
- * \param _measurement the measurement
- * \param _meas_covariance the noise of the measurement
- *
- */
- FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output);
-
- ~FeatureGnssFix() override{};
-
- private:
- GnssUtils::ComputePosOutput gnss_fix_output_;
-
+ public:
+ /** \brief Constructor from capture pointer and measure
+ *
+ * \param _measurement the measurement
+ * \param _meas_covariance the noise of the measurement
+ *
+ */
+ FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output);
+
+ ~FeatureGnssFix() override{};
+
+ private:
+ GnssUtils::ComputePosOutput gnss_fix_output_;
};
-inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output) :
- FeatureBase("FeatureGnssFix",
- (Eigen::Vector4d() << _gnss_fix_output.pos, _gnss_fix_output.rcv_bias(0)).finished(),
- _gnss_fix_output.sol_cov),
- gnss_fix_output_(_gnss_fix_output)
-{
- //
-};
+inline FeatureGnssFix::FeatureGnssFix(const GnssUtils::ComputePosOutput& _gnss_fix_output)
+ : FeatureBase("FeatureGnssFix",
+ (Eigen::Vector4d() << _gnss_fix_output.pos, _gnss_fix_output.rcv_bias(0)).finished(),
+ _gnss_fix_output.sol_cov),
+ gnss_fix_output_(_gnss_fix_output){
+ //
+ };
-} // namespace wolf
+} // namespace wolf
diff --git a/include/gnss/feature/feature_gnss_satellite.h b/include/gnss/feature/feature_gnss_satellite.h
index 20def2a87848dd3f2a769d713e44601d10908346..ded2d0cc9b8c11413fbba0b1328a926dc56ac208 100644
--- a/include/gnss/feature/feature_gnss_satellite.h
+++ b/include/gnss/feature/feature_gnss_satellite.h
@@ -20,57 +20,57 @@
#pragma once
-//Wolf includes
+// Wolf includes
#include "gnss/common/gnss.h"
#include "core/feature/feature_base.h"
-//std includes
+// std includes
#include "gnss_utils/range.h"
#include "gnss_utils/utils/satellite.h"
-namespace wolf {
-
+namespace wolf
+{
WOLF_PTR_TYPEDEFS(FeatureGnssSatellite);
WOLF_LIST_TYPEDEFS(FeatureGnssSatellite);
-//class FeatureGnssSatellite
+// class FeatureGnssSatellite
class FeatureGnssSatellite : public FeatureBase
{
- public:
-
- /** \brief Constructor
- *
- * \param _obs_sat satellite observation in rtklib format
- *
- * This constructor will take the pseudorange as measurement with 1 m² of variance
- *
- */
- FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::Range& _range);
-
- ~FeatureGnssSatellite() override{};
-
- const obsd_t& getObservation() const;
- int satNumber() const;
- const GnssUtils::Satellite& getSatellite() const;
- const GnssUtils::Range& getRange() const;
-
- private:
- obsd_t obs_sat_;
- GnssUtils::Satellite sat_;
- GnssUtils::Range range_;
+ public:
+ /** \brief Constructor
+ *
+ * \param _obs_sat satellite observation in rtklib format
+ *
+ * This constructor will take the pseudorange as measurement with 1 m² of variance
+ *
+ */
+ FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::Range& _range);
+
+ ~FeatureGnssSatellite() override{};
+
+ const obsd_t& getObservation() const;
+ int satNumber() const;
+ const GnssUtils::Satellite& getSatellite() const;
+ const GnssUtils::Range& getRange() const;
+
+ private:
+ obsd_t obs_sat_;
+ GnssUtils::Satellite sat_;
+ GnssUtils::Range range_;
};
-
-} // namespace wolf
+} // namespace wolf
// IMPLEMENTATION
-namespace wolf {
-
-inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat, const GnssUtils::Satellite& _sat, const GnssUtils::Range& _range) :
- FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_range.P_corrected), Eigen::Matrix1d(_range.P_var)),
- obs_sat_(_obs_sat),
- sat_(_sat),
- range_(_range)
+namespace wolf
+{
+inline FeatureGnssSatellite::FeatureGnssSatellite(const obsd_t& _obs_sat,
+ const GnssUtils::Satellite& _sat,
+ const GnssUtils::Range& _range)
+ : FeatureBase("FeatureGnssPseudoRange", Eigen::Vector1d(_range.P_corrected), Eigen::Matrix1d(_range.P_var)),
+ obs_sat_(_obs_sat),
+ sat_(_sat),
+ range_(_range)
{
//
}
@@ -95,4 +95,4 @@ inline const GnssUtils::Range& FeatureGnssSatellite::getRange() const
return range_;
}
-} // namespace wolf
+} // namespace wolf
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 624a75e7d9033cf54c1c5ef8794fee2082696e3e..9d73eeb36877025c79e8b1e593f6b8b352468501 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -31,19 +31,18 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorGnssFix);
// class
class ProcessorGnssFix : public ProcessorBase
{
protected:
- SensorGnssPtr sensor_gnss_;
- CaptureBasePtr last_KF_capture_, incoming_capture_;
- FeatureBasePtr last_KF_feature_, incoming_feature_;
- FrameBasePtr last_KF_;
- Eigen::Vector3d first_pos_;
- VectorComposite first_frame_state_;
+ SensorGnssPtr sensor_gnss_;
+ CaptureBasePtr last_KF_capture_, incoming_capture_;
+ FeatureBasePtr last_KF_feature_, incoming_feature_;
+ FrameBasePtr last_KF_;
+ Eigen::Vector3d first_pos_;
+ VectorComposite first_frame_state_;
public:
ProcessorGnssFix(const YAML::Node& _params);
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index 53e02eabca1569d4ad9fa951298732fcd40021df..3e1b2f4e2527704cdd05734d12e894064854e9e2 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -41,9 +41,9 @@ WOLF_PTR_TYPEDEFS(ProcessorGnssTdcp);
class ProcessorGnssTdcp : public ProcessorBase
{
protected:
- SensorGnssPtr sensor_gnss_;
- CaptureGnssPtr incoming_capture_;
- FrameBasePtr last_KF_;
+ SensorGnssPtr sensor_gnss_;
+ CaptureGnssPtr incoming_capture_;
+ FrameBasePtr last_KF_;
public:
ProcessorGnssTdcp(const YAML::Node& _params);
@@ -110,9 +110,9 @@ class ProcessorGnssTdcp : public ProcessorBase
private:
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr, FrameBasePtr _frm_ref);
- //PARAMS
+ // PARAMS
struct GnssUtils::TdcpOptions params_tdcp_;
- GnssUtils::Options params_compute_pos_;
+ GnssUtils::Options params_compute_pos_;
};
inline FrameBaseConstPtr ProcessorGnssTdcp::getLastKF() const
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index dc458eca1b22a2fd27dc21698edd774449c179d8..c22fcd058464289f4465bc8ebba3cd7211224e83 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -29,7 +29,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(ProcessorTrackerGnss);
// Class
@@ -153,7 +152,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
void removeOutliers(FactorBasePtrList fac_list, CaptureBasePtr cap);
- protected:
+ protected:
// PARAMS
GnssUtils::Options gnss_opt_;
GnssUtils::Options fix_opt_{GnssUtils::default_options};
@@ -163,7 +162,6 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
bool init_frames_, use_pseudo_ranges_, use_fix_, use_tdcp_, apply_sagnac_correction_;
int min_sbas_sats_;
std::string tdcp_structure_;
-
};
inline ProcessorTrackerGnss::~ProcessorTrackerGnss()
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index e5e380c79f21272d6e640d847cd6e3e0537d9c22..889bd3519b47690417c11f0b3d228b6fcd96e362 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -26,7 +26,6 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(SensorGnss);
class SensorGnss : public SensorBase
{
@@ -105,7 +104,6 @@ class SensorGnss : public SensorBase
namespace wolf
{
-
inline bool SensorGnss::isEnuDefined() const
{
return ENU_defined_;
diff --git a/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
index 6ff340a5db5624b988cb16cabb5ac7e3cb802b57..8b765f6aa58dc077eca562271655000f0f7620cf 100644
--- a/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
+++ b/include/gnss/tree_manager/tree_manager_sliding_window_tdcp.h
@@ -25,22 +25,21 @@
namespace wolf
{
-
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowTdcp)
class TreeManagerSlidingWindowTdcp : public TreeManagerSlidingWindow
{
- public:
- TreeManagerSlidingWindowTdcp(const YAML::Node& _params);
- WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowTdcp)
+ public:
+ TreeManagerSlidingWindowTdcp(const YAML::Node& _params);
+ WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowTdcp)
- ~TreeManagerSlidingWindowTdcp() override{}
+ ~TreeManagerSlidingWindowTdcp() override {}
- void keyFrameCallback(FrameBasePtr _key_frame) override;
+ void keyFrameCallback(FrameBasePtr _key_frame) override;
- protected:
- SensorBasePtr sensor_imu_;
- Eigen::Matrix6d cov_bias_;
+ protected:
+ SensorBasePtr sensor_imu_;
+ Eigen::Matrix6d cov_bias_;
};
} /* namespace wolf */
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
index ab0d5d3048cd3730dea84798d31c0d3cd638e207..6e99aa0f40cd0cada7706adbd1aee6ebb7209e30 100644
--- a/src/capture/capture_gnss.cpp
+++ b/src/capture/capture_gnss.cpp
@@ -21,29 +21,25 @@
#include "gnss/capture/capture_gnss.h"
#include <core/state_block/state_block_derived.h>
-namespace wolf {
-
-CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot) :
- CaptureBase("CaptureGnss", _ts, _sensor_ptr),
- snapshot_(_snapshot)
+namespace wolf
+{
+CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUtils::SnapshotPtr _snapshot)
+ : CaptureBase("CaptureGnss", _ts, _sensor_ptr), snapshot_(_snapshot)
{
// Clock bias
assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
- addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true));
+ addStateBlock('T', std::make_shared<StateParams1>(Vector1d(), true));
// interconstellation clock bias
assert(_sensor_ptr->getStateBlock('G') != nullptr);
- if(_sensor_ptr->isStateBlockDynamic('G'))
- addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true));
+ if (_sensor_ptr->isStateBlockDynamic('G')) addStateBlock('G', std::make_shared<StateParams1>(Vector1d(), true));
assert(_sensor_ptr->getStateBlock('E') != nullptr);
- if(_sensor_ptr->isStateBlockDynamic('E'))
- addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true));
-
+ if (_sensor_ptr->isStateBlockDynamic('E')) addStateBlock('E', std::make_shared<StateParams1>(Vector1d(), true));
}
CaptureGnss::~CaptureGnss()
{
- //std::cout << "Destroying GPS fix capture...\n";
+ // std::cout << "Destroying GPS fix capture...\n";
}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/capture/capture_gnss_fix.cpp b/src/capture/capture_gnss_fix.cpp
index 187b999f5df7a24b7806717f1e5ed1ec0d5ebc2a..ed306c9ff3619c0ab258f2bca35a9ecc32e9ba09 100644
--- a/src/capture/capture_gnss_fix.cpp
+++ b/src/capture/capture_gnss_fix.cpp
@@ -21,42 +21,37 @@
#include "gnss/capture/capture_gnss_fix.h"
#include <core/state_block/state_block_derived.h>
-
-namespace wolf {
-
-CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts,
- SensorBasePtr _sensor_ptr,
- const Eigen::Vector4d& _fix,
- const Eigen::Matrix4d& _covariance,
- bool _ecef_coordinates) :
- CaptureBase("CaptureGnssFix", _ts, _sensor_ptr),
- fix_ECEF_(_fix),
- covariance_ECEF_(_covariance)
+namespace wolf
+{
+CaptureGnssFix::CaptureGnssFix(const TimeStamp& _ts,
+ SensorBasePtr _sensor_ptr,
+ const Eigen::Vector4d& _fix,
+ const Eigen::Matrix4d& _covariance,
+ bool _ecef_coordinates)
+ : CaptureBase("CaptureGnssFix", _ts, _sensor_ptr), fix_ECEF_(_fix), covariance_ECEF_(_covariance)
{
- // Convert to ECEF from latlonalt
- if (not _ecef_coordinates)
- {
- fix_ECEF_.head<3>() = GnssUtils::latLonAltToEcef(_fix.head<3>());
- covariance_ECEF_.topLeftCorner<3,3>() = GnssUtils::enuToEcefCov(_fix.head<3>(),
- _covariance.topLeftCorner<3,3>());
- }
+ // Convert to ECEF from latlonalt
+ if (not _ecef_coordinates)
+ {
+ fix_ECEF_.head<3>() = GnssUtils::latLonAltToEcef(_fix.head<3>());
+ covariance_ECEF_.topLeftCorner<3, 3>() =
+ GnssUtils::enuToEcefCov(_fix.head<3>(), _covariance.topLeftCorner<3, 3>());
+ }
// Clock bias
assert(_sensor_ptr->getStateBlock('T') != nullptr and _sensor_ptr->isStateBlockDynamic('T'));
- addStateBlock('T', std::make_shared<StateParams1>(Vector1d(),true));
+ addStateBlock('T', std::make_shared<StateParams1>(Vector1d(), true));
// interconstellation clock bias
assert(_sensor_ptr->getStateBlock('G') != nullptr);
- if(_sensor_ptr->isStateBlockDynamic('G'))
- addStateBlock('G', std::make_shared<StateParams1>(Vector1d(),true));
+ if (_sensor_ptr->isStateBlockDynamic('G')) addStateBlock('G', std::make_shared<StateParams1>(Vector1d(), true));
assert(_sensor_ptr->getStateBlock('E') != nullptr);
- if(_sensor_ptr->isStateBlockDynamic('E'))
- addStateBlock('E', std::make_shared<StateParams1>(Vector1d(),true));
+ if (_sensor_ptr->isStateBlockDynamic('E')) addStateBlock('E', std::make_shared<StateParams1>(Vector1d(), true));
}
CaptureGnssFix::~CaptureGnssFix()
{
- //std::cout << "Destroying GPS fix capture...\n";
+ // std::cout << "Destroying GPS fix capture...\n";
}
-} //namespace wolf
+} // namespace wolf
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 440ad53de5da32bfb632d9edb528b210fc781267..bde2b6f039b740ca8cfcc956ef9c2c476b7121c3 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -27,7 +27,6 @@
namespace wolf
{
-
ProcessorGnssFix::ProcessorGnssFix(const YAML::Node& _params) : ProcessorBase("ProcessorGnssFix", 0, _params)
{
max_time_span_ = _params["keyframe_vote"]["max_time_span"].as<double>();
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 46bc2573bb5dfc8af02f5ee97b680713ea4a721f..9fb9144f14357ecbad693495a03bd6fcdf8d4da2 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -27,7 +27,6 @@
namespace wolf
{
-
ProcessorGnssTdcp::ProcessorGnssTdcp(const YAML::Node& _params) : ProcessorBase("ProcessorGnssTdcp", 0, _params)
{
params_tdcp_.min_common_sats = _params["tdcp"]["min_common_sats"].as<double>();
@@ -251,7 +250,7 @@ bool ProcessorGnssTdcp::voteForKeyFrame() const
// //std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
// //std::cout << "v_lastKF_origin: " << v_lastKF_origin.transpose() << std::endl;
// //std::cout << "v_lastKF_origin + v_origin_current: " << (v_lastKF_origin + v_origin_current).transpose() <<
- //std::endl;
+ // std::endl;
// if ((v_lastKF_origin + v_origin_current).norm() > params_tdcp_->dist_traveled)
// {
// WOLF_DEBUG("voting for KF: distance criterion");
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index ccf921e9baf8221b3324dfc098a8e1567072d1b1..b4b861d6c70904a00ec06e439eb41831b4d73570 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -40,86 +40,85 @@ ProcessorTrackerGnss::ProcessorTrackerGnss(const YAML::Node& _params)
inliers_tdcp_(0),
first_pos_(Eigen::Vector3d::Zero())
{
- max_time_span_ = _params["keyframe_vote"]["max_time_span"].as<double>();
- init_frames_ = _params["init_frames"].as<bool>();
- enu_map_fix_dist_ = _params["enu_map_fix_dist"].as<double>();
- remove_outliers_with_fix_ = _params["remove_outliers_with_fix"].as<bool>();
-
- remove_outliers_code_ = _params["code"]["remove_outliers"].as<bool>();
- if (remove_outliers_code_) outlier_residual_code_th_ = _params["code"]["outlier_residual_th"].as<double>();
-
- use_fix_ = _params["code"]["factor_type"].as<std::string>() == "batch";
- use_pseudo_ranges_ = _params["code"]["factor_type"].as<std::string>() == "satellite";
- min_sbas_sats_ = _params["gnss_options"]["min_sbas_sats"].as<int>();
- apply_sagnac_correction_ = _params["gnss_options"]["apply_sagnac_correction"].as<bool>();
-
- // GNSS OPTIONS (see gnss_utils.h)
- gnss_opt_.sateph = _params["gnss_options"]["sateph"].as<int>();
- gnss_opt_.ionoopt = _params["gnss_options"]["ionoopt"].as<int>();
- gnss_opt_.tropopt = _params["gnss_options"]["tropopt"].as<int>();
- gnss_opt_.sbascorr = _params["gnss_options"]["sbascorr"].as<int>();
- gnss_opt_.raim = _params["gnss_options"]["raim"].as<int>();
- gnss_opt_.elmin = _params["gnss_options"]["elmin"].as<double>();
- gnss_opt_.maxgdop = _params["gnss_options"]["maxgdop"].as<double>();
- gnss_opt_.GPS = _params["gnss_options"]["constellations"]["GPS"].as<bool>();
- gnss_opt_.SBS = _params["gnss_options"]["constellations"]["SBS"].as<bool>();
- gnss_opt_.GLO = _params["gnss_options"]["constellations"]["GLO"].as<bool>();
- gnss_opt_.GAL = _params["gnss_options"]["constellations"]["GAL"].as<bool>();
- gnss_opt_.QZS = _params["gnss_options"]["constellations"]["QZS"].as<bool>();
- gnss_opt_.CMP = _params["gnss_options"]["constellations"]["CMP"].as<bool>();
- gnss_opt_.IRN = _params["gnss_options"]["constellations"]["IRN"].as<bool>();
- gnss_opt_.LEO = _params["gnss_options"]["constellations"]["LEO"].as<bool>();
-
- // TDCP
- use_tdcp_ = _params["tdcp"]["factor_type"].as<std::string>() != "none";
- if (use_tdcp_)
+ max_time_span_ = _params["keyframe_vote"]["max_time_span"].as<double>();
+ init_frames_ = _params["init_frames"].as<bool>();
+ enu_map_fix_dist_ = _params["enu_map_fix_dist"].as<double>();
+ remove_outliers_with_fix_ = _params["remove_outliers_with_fix"].as<bool>();
+
+ remove_outliers_code_ = _params["code"]["remove_outliers"].as<bool>();
+ if (remove_outliers_code_) outlier_residual_code_th_ = _params["code"]["outlier_residual_th"].as<double>();
+
+ use_fix_ = _params["code"]["factor_type"].as<std::string>() == "batch";
+ use_pseudo_ranges_ = _params["code"]["factor_type"].as<std::string>() == "satellite";
+ min_sbas_sats_ = _params["gnss_options"]["min_sbas_sats"].as<int>();
+ apply_sagnac_correction_ = _params["gnss_options"]["apply_sagnac_correction"].as<bool>();
+
+ // GNSS OPTIONS (see gnss_utils.h)
+ gnss_opt_.sateph = _params["gnss_options"]["sateph"].as<int>();
+ gnss_opt_.ionoopt = _params["gnss_options"]["ionoopt"].as<int>();
+ gnss_opt_.tropopt = _params["gnss_options"]["tropopt"].as<int>();
+ gnss_opt_.sbascorr = _params["gnss_options"]["sbascorr"].as<int>();
+ gnss_opt_.raim = _params["gnss_options"]["raim"].as<int>();
+ gnss_opt_.elmin = _params["gnss_options"]["elmin"].as<double>();
+ gnss_opt_.maxgdop = _params["gnss_options"]["maxgdop"].as<double>();
+ gnss_opt_.GPS = _params["gnss_options"]["constellations"]["GPS"].as<bool>();
+ gnss_opt_.SBS = _params["gnss_options"]["constellations"]["SBS"].as<bool>();
+ gnss_opt_.GLO = _params["gnss_options"]["constellations"]["GLO"].as<bool>();
+ gnss_opt_.GAL = _params["gnss_options"]["constellations"]["GAL"].as<bool>();
+ gnss_opt_.QZS = _params["gnss_options"]["constellations"]["QZS"].as<bool>();
+ gnss_opt_.CMP = _params["gnss_options"]["constellations"]["CMP"].as<bool>();
+ gnss_opt_.IRN = _params["gnss_options"]["constellations"]["IRN"].as<bool>();
+ gnss_opt_.LEO = _params["gnss_options"]["constellations"]["LEO"].as<bool>();
+
+ // TDCP
+ use_tdcp_ = _params["tdcp"]["factor_type"].as<std::string>() != "none";
+ if (use_tdcp_)
+ {
+ tdcp_structure_ = _params["tdcp"]["structure"].as<std::string>();
+ remove_outliers_tdcp_ = _params["tdcp"]["remove_outliers"].as<bool>();
+ if (remove_outliers_tdcp_) outlier_residual_tdcp_th_ = _params["tdcp"]["outlier_residual_th"].as<double>();
+
+ gnss_opt_.carrier_opt.corr_iono = _params["tdcp"]["corr_iono"].as<bool>();
+ gnss_opt_.carrier_opt.corr_tropo = _params["tdcp"]["corr_tropo"].as<bool>();
+ gnss_opt_.carrier_opt.corr_clock = _params["tdcp"]["corr_clock"].as<bool>();
+ tdcp_params_.loss_function = _params["tdcp"]["apply_loss_function"].as<bool>();
+ tdcp_params_.time_window = _params["tdcp"]["time_window"].as<double>();
+ tdcp_params_.sigma_atm = _params["tdcp"]["sigma_atm"].as<double>();
+ tdcp_params_.sigma_carrier = _params["tdcp"]["sigma_carrier"].as<double>();
+ tdcp_params_.multi_freq = _params["tdcp"]["multi_freq"].as<bool>();
+ tdcp_params_.batch = _params["tdcp"]["factor_type"].as<std::string>() != "batch";
+ if (tdcp_params_.batch)
{
- tdcp_structure_ = _params["tdcp"]["structure"].as<std::string>();
- remove_outliers_tdcp_ = _params["tdcp"]["remove_outliers"].as<bool>();
- if (remove_outliers_tdcp_)
- outlier_residual_tdcp_th_ = _params["tdcp"]["outlier_residual_th"].as<double>();
-
- gnss_opt_.carrier_opt.corr_iono = _params["tdcp"]["corr_iono"].as<bool>();
- gnss_opt_.carrier_opt.corr_tropo = _params["tdcp"]["corr_tropo"].as<bool>();
- gnss_opt_.carrier_opt.corr_clock = _params["tdcp"]["corr_clock"].as<bool>();
- tdcp_params_.loss_function = _params["tdcp"]["apply_loss_function"].as<bool>();
- tdcp_params_.time_window = _params["tdcp"]["time_window"].as<double>();
- tdcp_params_.sigma_atm = _params["tdcp"]["sigma_atm"].as<double>();
- tdcp_params_.sigma_carrier = _params["tdcp"]["sigma_carrier"].as<double>();
- tdcp_params_.multi_freq = _params["tdcp"]["multi_freq"].as<bool>();
- tdcp_params_.batch = _params["tdcp"]["factor_type"].as<std::string>() != "batch";
- if (tdcp_params_.batch)
- {
- tdcp_params_.min_common_sats = _params["tdcp"]["min_common_sats"].as<int>();
- tdcp_params_.raim_n = _params["tdcp"]["raim_n"].as<int>();
- tdcp_params_.max_residual_ci = _params["tdcp"]["max_residual_ci"].as<double>();
- tdcp_params_.relinearize_jacobian = _params["tdcp"]["relinearize_jacobian"].as<bool>();
- tdcp_params_.max_iterations = _params["tdcp"]["max_iterations"].as<int>();
- tdcp_params_.residual_opt = _params["tdcp"]["residual_opt"].as<int>();
- }
-
- if (tdcp_structure_ != "all-all" and tdcp_structure_ != "consecutive" and tdcp_structure_ != "first-all")
- throw std::runtime_error(
- "unknown value for '/gnss/tdcp/structure', should be 'all-all', 'consecutive' or 'first-all'");
+ tdcp_params_.min_common_sats = _params["tdcp"]["min_common_sats"].as<int>();
+ tdcp_params_.raim_n = _params["tdcp"]["raim_n"].as<int>();
+ tdcp_params_.max_residual_ci = _params["tdcp"]["max_residual_ci"].as<double>();
+ tdcp_params_.relinearize_jacobian = _params["tdcp"]["relinearize_jacobian"].as<bool>();
+ tdcp_params_.max_iterations = _params["tdcp"]["max_iterations"].as<int>();
+ tdcp_params_.residual_opt = _params["tdcp"]["residual_opt"].as<int>();
}
- // COMPUTE FIX OPTIONS (RAIM)
- fix_opt_.raim = gnss_opt_.raim;
- fix_opt_.sateph = 6; // EPHOPT_SBAS2;
- fix_opt_.ionoopt = 9; // IONOPT_SBAS2;
- fix_opt_.tropopt = 2;
- fix_opt_.sbascorr = 15;
- fix_opt_.elmin = gnss_opt_.elmin;
- fix_opt_.maxgdop = gnss_opt_.maxgdop;
- // same constellations
- fix_opt_.GPS = gnss_opt_.GPS;
- fix_opt_.SBS = gnss_opt_.SBS;
- fix_opt_.GLO = gnss_opt_.GLO;
- fix_opt_.GAL = gnss_opt_.GAL;
- fix_opt_.QZS = gnss_opt_.QZS;
- fix_opt_.CMP = gnss_opt_.CMP;
- fix_opt_.IRN = gnss_opt_.IRN;
- fix_opt_.LEO = gnss_opt_.LEO;
+ if (tdcp_structure_ != "all-all" and tdcp_structure_ != "consecutive" and tdcp_structure_ != "first-all")
+ throw std::runtime_error(
+ "unknown value for '/gnss/tdcp/structure', should be 'all-all', 'consecutive' or 'first-all'");
+ }
+
+ // COMPUTE FIX OPTIONS (RAIM)
+ fix_opt_.raim = gnss_opt_.raim;
+ fix_opt_.sateph = 6; // EPHOPT_SBAS2;
+ fix_opt_.ionoopt = 9; // IONOPT_SBAS2;
+ fix_opt_.tropopt = 2;
+ fix_opt_.sbascorr = 15;
+ fix_opt_.elmin = gnss_opt_.elmin;
+ fix_opt_.maxgdop = gnss_opt_.maxgdop;
+ // same constellations
+ fix_opt_.GPS = gnss_opt_.GPS;
+ fix_opt_.SBS = gnss_opt_.SBS;
+ fix_opt_.GLO = gnss_opt_.GLO;
+ fix_opt_.GAL = gnss_opt_.GAL;
+ fix_opt_.QZS = gnss_opt_.QZS;
+ fix_opt_.CMP = gnss_opt_.CMP;
+ fix_opt_.IRN = gnss_opt_.IRN;
+ fix_opt_.LEO = gnss_opt_.LEO;
}
void ProcessorTrackerGnss::preProcess()
@@ -128,10 +127,8 @@ void ProcessorTrackerGnss::preProcess()
GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
GnssUtils::Options copy_opt = gnss_opt_;
- bool eph_sbas34 =
- gnss_opt_.sateph == EPHOPT_SBAS3 or gnss_opt_.sateph == EPHOPT_SBAS4;
- bool iono_sbas34 = gnss_opt_.ionoopt == IONOOPT_SBAS3 or
- gnss_opt_.ionoopt == IONOOPT_SBAS4;
+ bool eph_sbas34 = gnss_opt_.sateph == EPHOPT_SBAS3 or gnss_opt_.sateph == EPHOPT_SBAS4;
+ bool iono_sbas34 = gnss_opt_.ionoopt == IONOOPT_SBAS3 or gnss_opt_.ionoopt == IONOOPT_SBAS4;
#ifdef _WOLF_DEBUG
int n_initial = inc_snapshot->getObservations()->size();
@@ -151,8 +148,7 @@ void ProcessorTrackerGnss::preProcess()
* - Take the eventual discarded sats by RAIM
* - initialize clock bias
**/
- fix_incoming_ = GnssUtils::computePos(
- *inc_snapshot->getObservations(), *inc_snapshot->getNavigation(), fix_opt_);
+ fix_incoming_ = GnssUtils::computePos(*inc_snapshot->getObservations(), *inc_snapshot->getNavigation(), fix_opt_);
// Initialize clock bias stateblocks in capture
if (fix_incoming_.success)
@@ -283,8 +279,7 @@ void ProcessorTrackerGnss::preProcess()
for (auto sat : fix_incoming_.discarded_sats) discarded_rtklib_str += toString(sat) + " ";
for (auto ftr_pair : untracked_incoming_features_) detected_str += toString(ftr_pair.first) + " ";
for (auto sat_disc : discarded_gnssutils)
- if (fix_incoming_.discarded_sats.count(sat_disc) == 0)
- discarded_gnssutils_str += toString(sat_disc) + " ";
+ if (fix_incoming_.discarded_sats.count(sat_disc) == 0) discarded_gnssutils_str += toString(sat_disc) + " ";
#endif
WOLF_DEBUG("ProcessorTrackerGnss::preProcess()",
@@ -387,8 +382,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
// WOLF_DEBUG("ProcessorTrackerGnss::voteForKeyFrame");
// too old origin
- if (origin_ptr_ == nullptr or
- (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > max_time_span_)
+ if (origin_ptr_ == nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > max_time_span_)
{
WOLF_DEBUG("Vote for KF because of time span or null origin");
return true;
@@ -399,8 +393,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
* if it allows to add more satellites (untracked features in last is not empty)
*/
WOLF_DEBUG("Nbr. of active feature tracks: ", known_features_incoming_.size());
- if (known_features_incoming_.size() < min_features_for_keyframe_ and
- not untracked_last_features_.empty())
+ if (known_features_incoming_.size() < min_features_for_keyframe_ and not untracked_last_features_.empty())
{
WOLF_DEBUG("Vote for KF because of too less known_features_incoming and not empty untracked in last");
return true;
@@ -454,8 +447,8 @@ void ProcessorTrackerGnss::establishFactors()
{
GnssUtils::SnapshotPtr last_snapshot = std::static_pointer_cast<CaptureGnss>(last_ptr_)->getSnapshot();
- auto fix = GnssUtils::computePos(
- *last_snapshot->getObservations(), *last_snapshot->getNavigation(), gnss_opt_);
+ auto fix =
+ GnssUtils::computePos(*last_snapshot->getObservations(), *last_snapshot->getNavigation(), gnss_opt_);
if (fix.success)
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureGnssFix>(last_ptr_, fix);
@@ -487,12 +480,8 @@ void ProcessorTrackerGnss::establishFactors()
if (ftr_sat->getSatellite().sys == SYS_CMP) last_ptr_->getStateBlock('M')->unfix();
// Emplace a pseudo range factor
- auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
- ftr_sat,
- sensor_gnss_,
- shared_from_this(),
- apply_sagnac_correction_,
- applyLossFunction());
+ auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(
+ ftr_sat, ftr_sat, sensor_gnss_, shared_from_this(), apply_sagnac_correction_, applyLossFunction());
new_factors.push_back(new_fac);
WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
@@ -585,13 +574,8 @@ void ProcessorTrackerGnss::establishFactors()
FeatureBase::emplace<FeatureGnssDisplacement>(last_cap_gnss, tdcp_output.d, tdcp_output.cov_d);
// EMPLACE FACTOR
- last_fac_ptr = FactorBase::emplace<FactorGnssDisplacement3dWithClock>(ftr,
- ftr,
- ref_cap_gnss,
- sensor_gnss_,
- shared_from_this(),
- applyLossFunction(),
- factor_status);
+ last_fac_ptr = FactorBase::emplace<FactorGnssDisplacement3dWithClock>(
+ ftr, ftr, ref_cap_gnss, sensor_gnss_, shared_from_this(), applyLossFunction(), factor_status);
WOLF_DEBUG("TDCP BATCH factor ",
last_fac_ptr->id(),
@@ -669,8 +653,8 @@ void ProcessorTrackerGnss::establishFactors()
// emplace tdcp factor
auto factor_status = FAC_ACTIVE;
if (tdcp_structure_ == "first-all") factor_status = FAC_INACTIVE;
- double var_tdcp = dt * std::pow(tdcp_params_.sigma_atm, 2) +
- std::pow(tdcp_params_.sigma_carrier, 2);
+ double var_tdcp =
+ dt * std::pow(tdcp_params_.sigma_atm, 2) + std::pow(tdcp_params_.sigma_carrier, 2);
auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
sqrt(var_tdcp),
ftr_r,
@@ -698,8 +682,7 @@ void ProcessorTrackerGnss::establishFactors()
}
// remove outliers
- if (!new_factors.empty() and
- (remove_outliers_code_ or remove_outliers_tdcp_))
+ if (!new_factors.empty() and (remove_outliers_code_ or remove_outliers_tdcp_))
removeOutliers(new_factors, last_ptr_);
// getProblem()->print(4, 1, 1, 1);
@@ -736,8 +719,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
FactorBasePtrList remove_fac;
// If remove outliers with fix (ONLY PSEUDORANGE FACTORS)
- bool check_pseudoranges_with_fix =
- (cap == last_ptr_ and fix_last_.stat != 0 and remove_outliers_with_fix_);
+ bool check_pseudoranges_with_fix = (cap == last_ptr_ and fix_last_.stat != 0 and remove_outliers_with_fix_);
if (remove_outliers_code_ and check_pseudoranges_with_fix)
{
WOLF_DEBUG("Outliers for FactorPseudorange computed using fix_last");
@@ -887,8 +869,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
auto residual = fac->residual()(0);
// discard if residual too high
- double outlier_residual_th = is_pseudorange or is_fix ? outlier_residual_code_th_
- : outlier_residual_tdcp_th_;
+ double outlier_residual_th = is_pseudorange or is_fix ? outlier_residual_code_th_ : outlier_residual_tdcp_th_;
if (std::abs(residual) > outlier_residual_th)
{
WOLF_WARN("Discarding ", fac->getType(), " ", fac->id(), " considered OUTLIER");
@@ -929,8 +910,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
fac->remove();
}
WOLF_INFO_COND(not remove_fac.empty(), "ProcessorTrackerGnss::removeOutliers:")
- if (not remove_fac.empty() and (use_fix_ or use_pseudo_ranges_) and
- remove_outliers_code_)
+ if (not remove_fac.empty() and (use_fix_ or use_pseudo_ranges_) and remove_outliers_code_)
std::cout << "\n\tCode: " << outliers_code_ << "\t( "
<< (100.0 * outliers_code_) / (outliers_code_ + inliers_code_) << " \%)\n";
if (not remove_fac.empty() and use_tdcp_ and remove_outliers_tdcp_)
diff --git a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
index 7131f82ceee7533c8ef8d6bbea63ac0b57585985..23e80fe323bf1144fedc7dd2c510211fb39d2ce2 100644
--- a/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
+++ b/src/tree_manager/tree_manager_sliding_window_tdcp.cpp
@@ -22,7 +22,6 @@
namespace wolf
{
-
TreeManagerSlidingWindowTdcp::TreeManagerSlidingWindowTdcp(const YAML::Node& _params)
: TreeManagerSlidingWindow(_params)
{
diff --git a/test/gtest_example.cpp b/test/gtest_example.cpp
index 89a98d05a130887131130c74dcef112b272eff18..232accc4819f9f964142415c5e9268ce2a4a5060 100644
--- a/test/gtest_example.cpp
+++ b/test/gtest_example.cpp
@@ -22,19 +22,19 @@
TEST(TestTest, DummyTestExample)
{
- EXPECT_FALSE(false);
+ EXPECT_FALSE(false);
- ASSERT_TRUE(true);
+ ASSERT_TRUE(true);
- int my_int = 5;
+ int my_int = 5;
- ASSERT_EQ(my_int, 5);
+ ASSERT_EQ(my_int, 5);
-// PRINTF("All good at TestTest::DummyTestExample !\n");
+ // PRINTF("All good at TestTest::DummyTestExample !\n");
}
int main(int argc, char **argv)
{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
}
diff --git a/test/gtest_factor_gnss_displacement_3d.cpp b/test/gtest_factor_gnss_displacement_3d.cpp
index 5ea8d1f54cf869bf6c82b76bdb5f7e11762971e0..f90d99668208c251293d5a0912d3891cf7ac90e8 100644
--- a/test/gtest_factor_gnss_displacement_3d.cpp
+++ b/test/gtest_factor_gnss_displacement_3d.cpp
@@ -134,8 +134,7 @@ class FactorGnssDisplacement3dTest : public testing::Test
ASSERT_NEAR(gnss_sensor->getEnuMapRoll()->getState()(0), rpy_enu_map(0), Constants::EPS);
ASSERT_NEAR(gnss_sensor->getEnuMapPitch()->getState()(0), rpy_enu_map(1), Constants::EPS);
ASSERT_NEAR(gnss_sensor->getEnuMapYaw()->getState()(0), rpy_enu_map(2), Constants::EPS);
- ASSERT_MATRIX_APPROX(
- gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
+ ASSERT_MATRIX_APPROX(gnss_sensor->getEnuMapTranslation()->getState(), t_enu_map, Constants::EPS);
// Antena
ASSERT_MATRIX_APPROX(gnss_sensor->getP()->getState(), t_base_antena, Constants::EPS);
// Frames
@@ -166,7 +165,7 @@ class FactorGnssDisplacement3dTest : public testing::Test
WOLF_INFO("frame 2 created")
problem->keyFrameCallback(frame_2, nullptr);
WOLF_INFO("frame 2 informed")
-
+
// captures
cap_gnss_1 = CaptureBase::emplace<CaptureGnss>(frame_1, TimeStamp(0), gnss_sensor, nullptr);
cap_gnss_2 = CaptureBase::emplace<CaptureGnss>(frame_2, TimeStamp(1), gnss_sensor, nullptr);
diff --git a/test/gtest_factor_gnss_displacement_3d_with_clock.cpp b/test/gtest_factor_gnss_displacement_3d_with_clock.cpp
index 368c0cff957bc9d80d8563968c45a3f238f1c6dd..214c65995539811a724d16a1849d07efdc28a10b 100644
--- a/test/gtest_factor_gnss_displacement_3d_with_clock.cpp
+++ b/test/gtest_factor_gnss_displacement_3d_with_clock.cpp
@@ -274,7 +274,7 @@ TEST_F(FactorGnssDisplacement3dWithClockTest, gnss_map_base_2_position)
}
}
-/* Test with one GNSS displacement factor.
+/* Test with one GNSS displacement factor.
*
* Estimating: ENU_MAP yaw.
* Fixed: ENU_MAP position, MAP_BASE_1, MAP_BASE_2 and BASE_ANTENA.
diff --git a/test/gtest_factor_gnss_fix_3d_with_clock.cpp b/test/gtest_factor_gnss_fix_3d_with_clock.cpp
index 1bc863a038e3c7ffba37a2f6b7f89a5906e35af1..464398e8d7dc150817047e26ce3f4846065ec333 100644
--- a/test/gtest_factor_gnss_fix_3d_with_clock.cpp
+++ b/test/gtest_factor_gnss_fix_3d_with_clock.cpp
@@ -142,7 +142,7 @@ class FactorGnssFix3dWithClockTest : public testing::Test
// Solver
solver_ceres = std::static_pointer_cast<SolverCeres>(FactorySolverFile::create(
"SolverCeres", problem, gnss_dir + "/test/yaml/solver_ceres.yaml", {wolf_schema_dir}));
-
+
// frame
frame = problem->emplaceFrame(TimeStamp(0));
problem->keyFrameCallback(frame, nullptr);