diff --git a/include/gnss/factor/factor_gnss_pseudo_range.h b/include/gnss/factor/factor_gnss_pseudo_range.h
index ca6008e8f8cca136aa6ed9cb98e806075d325452..17aa7734cca56467daa075d31e748a0d014c31c8 100644
--- a/include/gnss/factor/factor_gnss_pseudo_range.h
+++ b/include/gnss/factor/factor_gnss_pseudo_range.h
@@ -40,12 +40,12 @@ class FactorGnssPseudoRange : public FactorAutodiff<FactorGnssPseudoRange, 1, 3,
_status,
_ftr_ptr->getFrame()->getP(),
_ftr_ptr->getFrame()->getO(),
- _ftr_ptr->getCapture()->getStateBlock("T"),
+ _ftr_ptr->getCapture()->getStateBlock('T'),
(_ftr_ptr->getSatellite().sys == SYS_GLO ?
- _ftr_ptr->getCapture()->getStateBlock("TG") :
+ _ftr_ptr->getCapture()->getStateBlock('G') :
(_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
+ _ftr_ptr->getCapture()->getStateBlock('E') :
+ _ftr_ptr->getCapture()->getStateBlock('M'))),//in case GPS, M is set but anyway not used
_sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(),
diff --git a/include/gnss/factor/factor_gnss_tdcp.h b/include/gnss/factor/factor_gnss_tdcp.h
index edc56b4ba211950e0417be1eac7753797b9d2149..0cf18f03d05bfcbf219e17ccc7ee59990cd3f827 100644
--- a/include/gnss/factor/factor_gnss_tdcp.h
+++ b/include/gnss/factor/factor_gnss_tdcp.h
@@ -41,10 +41,10 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
_status,
_ftr_r->getFrame()->getP(),
_ftr_r->getFrame()->getO(),
- _ftr_r->getCapture()->getStateBlock("T"),
+ _ftr_r->getCapture()->getStateBlock('T'),
_ftr_k->getFrame()->getP(),
_ftr_k->getFrame()->getO(),
- _ftr_k->getCapture()->getStateBlock("T"),
+ _ftr_k->getCapture()->getStateBlock('T'),
_sensor_gnss_ptr->getP(),
_sensor_gnss_ptr->getEnuMapTranslation(),
_sensor_gnss_ptr->getEnuMapRoll(),
@@ -56,8 +56,8 @@ class FactorGnssTdcp : public FactorAutodiff<FactorGnssTdcp, 1, 3, 4, 1, 3, 4, 1
satellite_ENU_r_(sensor_gnss_ptr_->getREnuEcef() * _ftr_r->getSatellite().pos + sensor_gnss_ptr_->gettEnuEcef())
{
assert(_ftr_r != _ftr_k);
- assert(_ftr_r->getCapture()->getStateBlock("T") != nullptr);
- assert(_ftr_k->getCapture()->getStateBlock("T") != nullptr);
+ assert(_ftr_r->getCapture()->getStateBlock('T') != nullptr);
+ assert(_ftr_k->getCapture()->getStateBlock('T') != nullptr);
WOLF_WARN_COND(!sensor_gnss_ptr_->isEnuDefined() && _status == FAC_ACTIVE, "Creating an FactorGnssTdcp without initializing ENU");
diff --git a/include/gnss/factor/factor_gnss_tdcp_2d.h b/include/gnss/factor/factor_gnss_tdcp_2d.h
index 65c0099f689fd9d273ef86485be6253ea1d8d9bc..3b78b2f06418396ecc33ff57b3a21da131d7707c 100644
--- a/include/gnss/factor/factor_gnss_tdcp_2d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_2d.h
@@ -19,7 +19,7 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
public:
- 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(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",
_ftr_ptr,
_frame_other_ptr,
@@ -46,7 +46,7 @@ class FactorGnssTdcp2d : public FactorAutodiff<FactorGnssTdcp2d, 3, 2, 1, 2, 1,
std::string getTopology() const override
{
- return std::string("MOTION");
+ return std::string("GEOM");
}
template<typename T>
diff --git a/include/gnss/factor/factor_gnss_tdcp_3d.h b/include/gnss/factor/factor_gnss_tdcp_3d.h
index 90793fd9b5806e10acb0af1b286f70704b027442..90df1ecfe2b6eacc759f2f6e3dd3b78bfc2edf69 100644
--- a/include/gnss/factor/factor_gnss_tdcp_3d.h
+++ b/include/gnss/factor/factor_gnss_tdcp_3d.h
@@ -19,24 +19,24 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
public:
- 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(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()),
+ _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");
@@ -46,7 +46,7 @@ class FactorGnssTdcp3d : public FactorAutodiff<FactorGnssTdcp3d, 3, 3, 4, 3, 4,
std::string getTopology() const override
{
- return std::string("MOTION");
+ return std::string("GEOM");
}
template<typename T>
diff --git a/include/gnss/factor/factor_gnss_tdcp_batch.h b/include/gnss/factor/factor_gnss_tdcp_batch.h
new file mode 100644
index 0000000000000000000000000000000000000000..e525ab995d90e641c3a266789b43272f03bfd7e6
--- /dev/null
+++ b/include/gnss/factor/factor_gnss_tdcp_batch.h
@@ -0,0 +1,120 @@
+
+#ifndef FACTOR_GNSS_TDCP_BATCH_H_
+#define FACTOR_GNSS_TDCP_BATCH_H_
+
+//Wolf includes
+#include "core/common/wolf.h"
+#include "core/factor/factor_autodiff.h"
+#include "gnss/sensor/sensor_gnss.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGnssTdcpBatch);
+
+class FactorGnssTdcpBatch : public FactorAutodiff<FactorGnssTdcpBatch, 4, 3, 4, 1, 3, 4, 1, 3, 1, 1, 1>
+{
+ protected:
+ SensorGnssPtr sensor_gnss_ptr_;
+
+ public:
+
+ FactorGnssTdcpBatch(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",
+ _ftr_ptr,
+ _capture_other_ptr->getFrame(),
+ _capture_other_ptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _capture_other_ptr->getFrame()->getP(),
+ _capture_other_ptr->getFrame()->getO(),
+ _capture_other_ptr->getStateBlock('T'),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getCapture()->getStateBlock('T'),
+ _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");
+ }
+
+ ~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,
+ const T* const _t1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _t2,
+ 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;
+
+};
+
+template<typename T>
+inline bool FactorGnssTdcpBatch::operator ()(const T* const _x1,
+ const T* const _o1,
+ const T* const _t1,
+ const T* const _x2,
+ const T* const _o2,
+ const T* const _t2,
+ 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);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_MAP_BASE2(_x2);
+ Eigen::Map<const Eigen::Quaternion<T>> q_MAP_BASE2(_o2);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> t_BASE_ANTENA(_x_antena);
+ 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]);
+
+ // Expected displacement
+ Eigen::Matrix<T,3,1> expected_ECEF = R_ECEF_ENU * R_ENU_MAP * ((q_MAP_BASE2 * t_BASE_ANTENA + t_MAP_BASE2) - (q_MAP_BASE1 * t_BASE_ANTENA + t_MAP_BASE1));
+
+ // position error
+ Eigen::Matrix<T,3,1> error_ECEF = expected_ECEF - getMeasurement().head<3>().cast<T>();
+
+ // clock error
+ T error_clock = _t2 - _t1;
+
+ // Compute residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * (expected_ECEF - getMeasurement().cast<T>());
+
+ //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;
+ //std::cout << "antena: " << _x_antena[0] << " " << _x_antena[1] << " " << _x_antena[2] << std::endl;
+ //std::cout << "RPY: " << _roll[0] << " " << _pitch[1] << " " << _yaw[2] << std::endl;
+ //std::cout << "expected_ECEF: " << expected_ECEF[0] << " " << expected_ECEF[1] << " " << expected_ECEF[2] << std::endl;
+ //std::cout << "getMeasurement(): " << getMeasurement().transpose << std::endl;
+
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/gnss/feature/feature_gnss_tdcp.h b/include/gnss/feature/feature_gnss_tdcp.h
index 543c4e933fd1adb0a444bd76b93227ac2e7c17f9..7168044696301f1b4e85612754dbb1edd038b7d6 100644
--- a/include/gnss/feature/feature_gnss_tdcp.h
+++ b/include/gnss/feature/feature_gnss_tdcp.h
@@ -23,6 +23,9 @@ class FeatureGnssTdcp : public FeatureBase
FeatureGnssTdcp(const Eigen::Vector4d& _measurement, const Eigen::Matrix4d& _meas_covariance) :
FeatureBase("FeatureGnssTdcp", _measurement, _meas_covariance)
{};
+ FeatureGnssTdcp(const Eigen::Vector3d& _measurement, const Eigen::Matrix3d& _meas_covariance) :
+ FeatureBase("FeatureGnssTdcp", _measurement, _meas_covariance)
+ {};
~FeatureGnssTdcp() override{};
};
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index 39223788b660132dcaa79af840539258a8a53f70..915a8c0e8c5128ea11cb629c4755be0bec942afc 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -18,25 +18,35 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorGnssFix);
struct ParamsProcessorGnssFix : public ParamsProcessorBase
{
- bool fix_from_raw;
+ bool fix_from_raw, init_enu_map, remove_outliers;
GnssUtils::Options compute_pos_opt;
double max_time_span;
double dist_traveled;
- double enu_map_init_dist_min;
+ double enu_map_init_dist_min, enu_map_init_dist_max;
+ double enu_map_fix_dist;
+ double outlier_residual_th;
ParamsProcessorGnssFix() = default;
ParamsProcessorGnssFix(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorBase(_unique_name, _server)
{
- max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
- dist_traveled = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/dist_traveled");
- enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min");
- fix_from_raw = _server.getParam<bool> (prefix + _unique_name + "/fix_from_raw");
+ max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ dist_traveled = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/dist_traveled");
+ init_enu_map = _server.getParam<bool> (prefix + _unique_name + "/init_enu_map");
+ if (init_enu_map)
+ {
+ enu_map_init_dist_min = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_min");
+ enu_map_init_dist_max = _server.getParam<double> (prefix + _unique_name + "/enu_map_init_dist_max");
+ }
+ enu_map_fix_dist = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_dist");
+ fix_from_raw = _server.getParam<bool> (prefix + _unique_name + "/fix_from_raw");
+ remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
+ outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
// COMPUTE POS PARAMS (only if compute fix from yaw)
if (fix_from_raw)
{
- // GNSS OPTIONS (see rtklib.h)
+ // GNSS OPTIONS (see gnss_utils.h)
compute_pos_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris
compute_pos_opt.ionoopt = _server.getParam<int> (prefix + _unique_name + "/gnss/ionoopt"); // ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode)
compute_pos_opt.tropopt = _server.getParam<int> (prefix + _unique_name + "/gnss/tropopt"); // troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction)
@@ -59,11 +69,16 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
std::string print() const
{
return "\n" + ParamsProcessorBase::print() + "\n" +
- "max_time_span: " + std::to_string(max_time_span) + "\n" +
- "dist_traveled: " + std::to_string(dist_traveled) + "\n" +
- "fix_from_raw: " + std::to_string(fix_from_raw) + "\n" +
- "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n" +
- "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n" +
+ "max_time_span: " + std::to_string(max_time_span) + "\n" +
+ "dist_traveled: " + std::to_string(dist_traveled) + "\n" +
+ "fix_from_raw: " + std::to_string(fix_from_raw) + "\n" +
+ "init_enu_map: " + std::to_string(init_enu_map) + "\n" +
+ (init_enu_map ?
+ "enu_map_init_dist_min: "+ std::to_string(enu_map_init_dist_min) + "\n" : "") +
+ "enu_map_fix_dist: " + std::to_string(enu_map_fix_dist) + "\n" +
+ "remove_outliers: " + std::to_string(remove_outliers) + "\n" +
+ "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n" +
+ "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n" +
(fix_from_raw ?
"gnss/sateph: " + std::to_string(compute_pos_opt.sateph) + "\n" +
"gnss/ionoopt: " + std::to_string(compute_pos_opt.ionoopt) + "\n" +
@@ -90,10 +105,13 @@ class ProcessorGnssFix : public ProcessorBase
protected:
SensorGnssPtr sensor_gnss_;
ParamsProcessorGnssFixPtr params_gnss_;
- CaptureBasePtr first_capture_, last_KF_capture_, incoming_capture_;
- FeatureGnssFixPtr first_feature_, last_KF_feature_;
+ CaptureBasePtr last_KF_capture_, incoming_capture_;
+ FeatureGnssFixPtr last_KF_feature_;
FrameBasePtr last_KF_;
GnssUtils::ComputePosOutput incoming_pos_out_;
+ Eigen::Vector3d first_pos_;
+ VectorComposite first_frame_state_;
+
public:
ProcessorGnssFix(ParamsProcessorGnssFixPtr _params);
@@ -144,7 +162,7 @@ class ProcessorGnssFix : public ProcessorBase
private:
FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
- bool rejectOutlier(FactorBasePtr ctr_ptr);
+ bool detectOutlier(FactorBasePtr ctr_ptr);
};
diff --git a/include/gnss/processor/processor_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index 45e98bf204d0ba1f22920ad8cff807e860ae55c7..51b2fe8824ad6bb53d1cecaa1ca55e322f1eae9f 100644
--- a/include/gnss/processor/processor_tracker_gnss.h
+++ b/include/gnss/processor/processor_tracker_gnss.h
@@ -6,6 +6,7 @@
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/feature/feature_gnss_satellite.h"
#include "gnss_utils/gnss_utils.h"
+#include "gnss_utils/tdcp.h"
namespace wolf
{
@@ -16,26 +17,31 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
{
GnssUtils::Options gnss_opt;
GnssUtils::Options fix_opt{GnssUtils::default_options};
+ GnssUtils::TdcpBatchParams tdcp_batch_params;
double max_time_span;
- bool remove_outliers, remove_outliers_tdcp;
+ bool remove_outliers, remove_outliers_tdcp, remove_outliers_with_fix;
double outlier_residual_th;
- bool init_frames;
+ bool init_frames, pseudo_ranges;
+ double enu_map_fix_dist;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
ParamsProcessorTrackerFeature(_unique_name, _server)
{
- remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
- outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
- init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames");
- max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ remove_outliers = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers");
+ remove_outliers_with_fix = _server.getParam<bool> (prefix + _unique_name + "/remove_outliers_with_fix");
+ outlier_residual_th = _server.getParam<double> (prefix + _unique_name + "/outlier_residual_th");
+ init_frames = _server.getParam<bool> (prefix + _unique_name + "/init_frames");
+ max_time_span = _server.getParam<double> (prefix + _unique_name + "/keyframe_vote/max_time_span");
+ enu_map_fix_dist = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_dist");
+ pseudo_ranges = _server.getParam<bool> (prefix + _unique_name + "/pseudo_ranges");
// GNSS OPTIONS (see rtklib.h)
gnss_opt.sateph = _server.getParam<int> (prefix + _unique_name + "/gnss/sateph"); // satellite ephemeris/clock (0:broadcast ephemeris,1:precise ephemeris,2:broadcast + SBAS,3:ephemeris option: broadcast + SSR_APC,4:broadcast + SSR_COM,5: QZSS LEX ephemeris
gnss_opt.ionoopt = _server.getParam<int> (prefix + _unique_name + "/gnss/ionoopt"); // ionosphere option (0:correction off,1:broadcast mode,2:SBAS model,3:L1/L2 or L1/L5,4:estimation,5:IONEX TEC model,6:QZSS broadcast,7:QZSS LEX ionosphere,8:SLANT TEC mode)
gnss_opt.tropopt = _server.getParam<int> (prefix + _unique_name + "/gnss/tropopt"); // troposphere option: (0:correction off,1:Saastamoinen model,2:SBAS model,3:troposphere option: ZTD estimation,4:ZTD+grad estimation,5:ZTD correction,6:ZTD+grad correction)
gnss_opt.sbascorr = _server.getParam<int> (prefix + _unique_name + "/gnss/sbascorr");// SBAS option (1:long term correction,2:fast correction,4:ionosphere correction,8:ranging)
- gnss_opt.raim = _server.getParam<bool> (prefix + _unique_name + "/gnss/raim"); // RAIM enabled
+ gnss_opt.raim = _server.getParam<int> (prefix + _unique_name + "/gnss/raim"); // RAIM enabled
gnss_opt.elmin = D2R * _server.getParam<double>(prefix + _unique_name + "/gnss/elmin"); // min elevation (degrees)
gnss_opt.maxgdop = _server.getParam<double>(prefix + _unique_name + "/gnss/maxgdop"); // maxgdop: reject threshold of gdop
@@ -53,6 +59,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
if (gnss_opt.tdcp.enabled)
{
remove_outliers_tdcp = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/remove_outliers");
+ gnss_opt.tdcp.batch = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/batch");
gnss_opt.tdcp.corr_iono = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_iono");
gnss_opt.tdcp.corr_tropo = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/corr_tropo");
gnss_opt.tdcp.loss_function = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/loss_function");
@@ -60,6 +67,16 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
gnss_opt.tdcp.sigma_atm = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_atm");
gnss_opt.tdcp.sigma_carrier = _server.getParam<double>(prefix + _unique_name + "/gnss/tdcp/sigma_carrier");
gnss_opt.tdcp.multi_freq = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/multi_freq");
+ if (gnss_opt.tdcp.batch)
+ {
+ tdcp_batch_params.min_common_sats = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/min_common_sats");
+ tdcp_batch_params.raim_n = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/raim_n");
+ tdcp_batch_params.raim_min_residual = _server.getParam<double> (prefix + _unique_name + "/gnss/tdcp/raim_min_residual");
+ tdcp_batch_params.relinearize_jacobian = _server.getParam<bool> (prefix + _unique_name + "/gnss/tdcp/relinearize_jacobian");
+ tdcp_batch_params.max_iterations = _server.getParam<int> (prefix + _unique_name + "/gnss/tdcp/max_iterations");
+ tdcp_batch_params.tdcp.multi_freq = gnss_opt.tdcp.multi_freq;
+ tdcp_batch_params.tdcp = gnss_opt.tdcp;
+ }
}
// COMPUTE FIX OPTIONS (RAIM)
@@ -73,6 +90,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "remove_outliers: " + std::to_string(remove_outliers) + "\n"
+ "outlier_residual_th: " + std::to_string(outlier_residual_th) + "\n"
+ "init_frames: " + std::to_string(init_frames) + "\n"
+ + "enu_map_fix_dist: " + std::to_string(enu_map_fix_dist) + "\n"
+ "keyframe_vote/max_time_span: " + std::to_string(max_time_span) + "\n"
+ "gnss/sateph: " + std::to_string(gnss_opt.sateph) + "\n"
+ "gnss/ionoopt: " + std::to_string(gnss_opt.ionoopt) + "\n"
@@ -90,6 +108,7 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
+ "gnss/constellations/IRN: " + std::to_string(gnss_opt.IRN) + "\n"
+ "gnss/constellations/LEO: " + std::to_string(gnss_opt.LEO) + "\n"
+ "gnss/tdcp/enabled: " + std::to_string(gnss_opt.tdcp.enabled) + "\n"
+ + "gnss/tdcp/batch: " + std::to_string(gnss_opt.tdcp.batch) + "\n"
+ "gnss/tdcp/corr_iono: " + std::to_string(gnss_opt.tdcp.corr_iono) + "\n"
+ "gnss/tdcp/corr_tropo: " + std::to_string(gnss_opt.tdcp.corr_tropo) + "\n"
+ "gnss/tdcp/loss_function: " + std::to_string(gnss_opt.tdcp.loss_function) + "\n"
@@ -124,6 +143,7 @@ class ProcessorTrackerGnss : public ProcessorTrackerFeature
GnssUtils::ComputePosOutput fix_incoming_, fix_last_;
unsigned int outliers_pseudorange_, outliers_tdcp_, inliers_pseudorange_, inliers_tdcp_;
std::map<int, unsigned int> sat_outliers_;
+ Eigen::Vector3d first_pos_;
/** \brief Track provided features in \b _capture
* \param _features_in input list of features in \b last to track
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 3b8fcccbde2c35e8f9bb0dee36e076b8ef9d1175..0fe76e0d808b0389b3b4da461aab3291dc26c6a1 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -8,27 +8,41 @@
namespace wolf {
+static const char CLOCK_BIAS_KEY = 'T';
+static const char CLOCK_BIAS_GPS_GLO_KEY = 'G';
+static const char CLOCK_BIAS_GPS_GAL_KEY = 'E';
+static const char CLOCK_BIAS_GPS_CMP_KEY = 'M';
+
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorGnss);
WOLF_PTR_TYPEDEFS(SensorGnss);
struct ParamsSensorGnss : public ParamsSensorBase
{
- // add GNSS parameters here
- std::string ENU_mode;
- bool extrinsics_fixed;
- bool roll_fixed = false;
- bool pitch_fixed = false;
- bool yaw_fixed = false;
- bool translation_fixed = false;
+ // extrinsics and intrinsics
+ bool extrinsics_fixed = true;
+ bool clock_bias_GPS_GLO_dynamic = false;
+ bool clock_bias_GPS_GAL_dynamic = false;
+ bool clock_bias_GPS_CMP_dynamic = false;
+
+ // ENU
+ std::string ENU_mode = "auto";
+ bool roll_fixed = true;
+ bool pitch_fixed = true;
+ bool yaw_fixed = true;
+ bool translation_fixed = true;
Eigen::Vector3d ENU_latlonalt = Eigen::Vector3d::Zero();
+
~ParamsSensorGnss() override = default;
ParamsSensorGnss() = default;
ParamsSensorGnss(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
- extrinsics_fixed = _server.getParam<bool>(prefix + _unique_name + "/extrinsics_fixed");
- ENU_mode = _server.getParam<std::string>(prefix + _unique_name + "/ENU/mode");
+ extrinsics_fixed = _server.getParam<bool>(prefix + _unique_name + "/extrinsics_fixed");
+ clock_bias_GPS_GLO_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GLO_dynamic");
+ clock_bias_GPS_GAL_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_GAL_dynamic");
+ clock_bias_GPS_CMP_dynamic = _server.getParam<bool>(prefix + _unique_name + "/clock_bias_GPS_CMP_dynamic");
+ ENU_mode = _server.getParam<std::string>(prefix + _unique_name + "/ENU/mode");
if (ENU_mode == "manual" or ENU_mode == "auto")
{
roll_fixed = _server.getParam<bool>(prefix + _unique_name + "/ENU/roll_fixed");
@@ -45,14 +59,17 @@ struct ParamsSensorGnss : public ParamsSensorBase
}
std::string print() const
{
- return "\n" + ParamsSensorBase::print() + "\n"
- + "extrinsics_fixed: " + std::to_string(extrinsics_fixed) + "\n"
- + "ENU_mode: " + ENU_mode + "\n"
- + "roll_fixed: " + std::to_string(roll_fixed) + "\n"
- + "pitch_fixed: " + std::to_string(pitch_fixed ) + "\n"
- + "yaw_fixed: " + std::to_string(yaw_fixed) + "\n"
- + "translation_fixed: " + std::to_string(translation_fixed) + "\n"
- + "ENU_latlonalt: to_string not implemented yet!" + "\n";
+ return "\n" + ParamsSensorBase::print() + "\n"
+ + "extrinsics_fixed: " + std::to_string(extrinsics_fixed) + "\n"
+ + "clock_bias_GPS_GLO_dynamic: " + std::to_string(clock_bias_GPS_GLO_dynamic) + "\n"
+ + "clock_bias_GPS_GAL_dynamic: " + std::to_string(clock_bias_GPS_GAL_dynamic) + "\n"
+ + "clock_bias_GPS_CMP_dynamic: " + std::to_string(clock_bias_GPS_CMP_dynamic) + "\n"
+ + "ENU_mode: " + ENU_mode + "\n"
+ + "roll_fixed: " + std::to_string(roll_fixed) + "\n"
+ + "pitch_fixed: " + std::to_string(pitch_fixed ) + "\n"
+ + "yaw_fixed: " + std::to_string(yaw_fixed) + "\n"
+ + "translation_fixed: " + std::to_string(translation_fixed) + "\n"
+ + "ENU_latlonalt: to_string not implemented yet!" + "\n";
}
};
@@ -86,6 +103,7 @@ class SensorGnss : public SensorBase
bool isEnuMapRotationInitialized() const;
bool isEnuModeEcef() const;
bool isEnuModeAuto() const;
+ bool isEnuMapFixed() const;
// Sets
void setEnuMapTranslationState(const Eigen::Vector3d& t_ENU_MAP);
@@ -97,6 +115,7 @@ class SensorGnss : public SensorBase
void setEnuMapInitialized(const bool& _init);
void setEnuMapTranslationInitialized(const bool& _init);
void setEnuMapRotationInitialized(const bool& _init);
+ void fixEnuMap();
// compute
template<typename T>
@@ -139,22 +158,22 @@ inline bool SensorGnss::isEnuModeAuto() const
inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
{
- return getStateBlock("t");
+ return getStateBlock('t');
}
inline StateBlockPtr SensorGnss::getEnuMapRoll() const
{
- return getStateBlock("r");
+ return getStateBlock('r');
}
inline StateBlockPtr SensorGnss::getEnuMapPitch() const
{
- return getStateBlock("p");
+ return getStateBlock('p');
}
inline StateBlockPtr SensorGnss::getEnuMapYaw() const
{
- return getStateBlock("y");
+ return getStateBlock('y');
}
inline const Eigen::Matrix3d& SensorGnss::getREnuEcef() const
@@ -203,6 +222,22 @@ inline void SensorGnss::setEnuMapRotationInitialized(const bool& _init)
R_ENU_MAP_initialized_ = _init;
}
+inline bool SensorGnss::isEnuMapFixed() const
+{
+ return getEnuMapTranslation()->isFixed() and
+ getEnuMapRoll()->isFixed() and
+ getEnuMapPitch()->isFixed() and
+ getEnuMapYaw()->isFixed();
+}
+
+inline void SensorGnss::fixEnuMap()
+{
+ getEnuMapTranslation()->fix();
+ getEnuMapRoll()->fix();
+ getEnuMapPitch()->fix();
+ getEnuMapYaw()->fix();
+}
+
} // namespace wolf
#endif //SENSOR_GPS_H_
diff --git a/src/capture/capture_gnss.cpp b/src/capture/capture_gnss.cpp
index 5e49047fc99f3a69c4a54b16d1faba2e3db81d63..dba80b6658399b6dc39d82ada38afc5624a446e1 100644
--- a/src/capture/capture_gnss.cpp
+++ b/src/capture/capture_gnss.cpp
@@ -7,7 +7,21 @@ CaptureGnss::CaptureGnss(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, GnssUt
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<StateBlock>(1,true), nullptr);
+
+ // interconstellation clock bias
+ assert(_sensor_ptr->getStateBlock('G') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('G'))
+ addStateBlock('G', std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock('E') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('E'))
+ addStateBlock('E', std::make_shared<StateBlock>(1,true), nullptr);
+ assert(_sensor_ptr->getStateBlock('M') != nullptr);
+ if(_sensor_ptr->isStateBlockDynamic('M'))
+ addStateBlock('M', std::make_shared<StateBlock>(1,true), nullptr);
+
}
CaptureGnss::~CaptureGnss()
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 69ab6723a94f20881a38e4479ea694a3128bda69..b2ad0211aa2cb1ba52b112e8980bb47aa9fcc4cf 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -10,8 +10,7 @@ namespace wolf
ProcessorGnssFix::ProcessorGnssFix(ParamsProcessorGnssFixPtr _params_gnss) :
ProcessorBase("ProcessorGnssFix", 0, _params_gnss),
- params_gnss_(_params_gnss),
- first_capture_(nullptr)
+ params_gnss_(_params_gnss)
{
//
}
@@ -48,29 +47,6 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
FrameBasePtr new_frame = nullptr;
FactorBasePtr new_fac = nullptr;
- // ALREADY CREATED KF
- PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
- if (KF_pack and (last_KF_capture_==nullptr or KF_pack->key_frame != last_KF_capture_->getFrame()))
- {
- WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
- new_frame = KF_pack->key_frame;
- }
- // MAKE KF
- else if (permittedKeyFrame() && voteForKeyFrame())
- {
- WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp());
- new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp());
- KF_created = true;
- }
-
- // ESTABLISH FACTOR
- if (new_frame == nullptr)
- return;
-
- // ESTABLISH FACTOR
- // link capture
- incoming_capture_->link(new_frame);
-
// emplace features
if (israw)
{
@@ -91,28 +67,45 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
incoming_pos_out_.pos = fix_capture->getPositionEcef(); // Eigen::Vector3d pos; // position (m)
incoming_pos_out_.vel = Eigen::Vector3d::Zero(); // Eigen::Vector3d vel; // velocity (m/s)
incoming_pos_out_.pos_covar = fix_capture->getCovarianceEcef(); // Eigen::Matrix3d pos_covar; // position covariance (m^2)
- //incoming_pos_out_.rcv_bias = Eigen::Vector1d::Zero(); // Eigen::VectorXd rcv_bias; // receiver clock bias to time systems (s)
incoming_pos_out_.type = 0; // int type; // coordinates used (0:xyz-ecef,1:enu-baseline)
- //incoming_pos_out_.stat = 0; // int stat; // solution status (SOLQ_???)
- //incoming_pos_out_.ns = 0; // int ns; // number of valid satellites
- //incoming_pos_out_.age = 0.0; // double age; // age of differential (s)
- //incoming_pos_out_.ratio = 0.0; // double ratio; // AR ratio factor for valiation
- //incoming_pos_out_.pos_stat = 0; // int pos_stat; // return from pntpos
- //incoming_pos_out_.lat_lon = Eigen::Vector3d::Zero(); // Eigen::Vector3d lat_lon; // latitude_longitude_altitude
}
auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+ // ALREADY CREATED KF
+ PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( incoming_capture_, params_gnss_->time_tolerance);
+ if (KF_pack and last_KF_capture_ and KF_pack->key_frame == last_KF_capture_->getFrame())
+ KF_pack = nullptr;
+ if (KF_pack)
+ {
+ WOLF_DEBUG("PR ", getName()," - capture ", incoming_capture_->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
+ new_frame = KF_pack->key_frame;
+ }
+ // MAKE KF
+ else if (permittedKeyFrame() && voteForKeyFrame())
+ {
+ WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp());
+ new_frame = getProblem()->emplaceKeyFrame(incoming_capture_->getTimeStamp());
+ KF_created = true;
+ }
+ // OTHERWISE return
+ else
+ return;
+
+ // ESTABLISH FACTOR
+ // link capture
+ incoming_capture_->link(new_frame);
// emplace factor
new_fac = emplaceFactor(incoming_feature);
- // outlier rejection (only can be evaluated if ENU defined and ENU-MAP initialized)
- WOLF_DEBUG("ProcessorGnssFix: outlier rejection");
- if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
- if (rejectOutlier(new_fac))
- {
- new_fac->remove();
- return;
- }
+ // outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
+ if (params_gnss_->remove_outliers and
+ sensor_gnss_->isEnuDefined() and
+ sensor_gnss_->isEnuMapFixed() and
+ detectOutlier(new_fac))
+ {
+ new_frame->remove();
+ return;
+ }
// store last KF
last_KF_capture_ = incoming_capture_;
@@ -126,26 +119,35 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
}
// Store the first capture that established a factor (for initializing ENU-MAP)
- if (first_capture_ == nullptr)
+ if (first_frame_state_.empty() and
+ not sensor_gnss_->isEnuMapFixed())
{
- first_capture_ = incoming_capture_;
- first_feature_ = incoming_feature;
+ first_frame_state_ = incoming_capture_->getFrame()->getState();
+ first_pos_ = incoming_feature->getMeasurement();
}
- // Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized
- if (!first_capture_->isRemoving())
+ // Initialize ENU-MAP if: ENU defined and ENU-MAP not initialized (and not fixed) and far enough
+ if (params_gnss_->init_enu_map and
+ not first_frame_state_.empty() and
+ sensor_gnss_->isEnuDefined() and
+ not sensor_gnss_->isEnuMapInitialized() and
+ not sensor_gnss_->isEnuMapFixed() and
+ (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
{
- assert(first_capture_->getFrame() != nullptr && incoming_capture_->getFrame() != nullptr);
- if ( sensor_gnss_->isEnuDefined() && !sensor_gnss_->isEnuMapInitialized() )
- {
- WOLF_DEBUG("(re-)initializing enu map");
- sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState().vector("PO"), first_feature_->getMeasurement(),
- incoming_capture_->getFrame()->getState().vector("PO"), incoming_feature->getMeasurement());
- // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_min)
- if ((first_feature_->getMeasurement() - incoming_feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_min)
- sensor_gnss_->setEnuMapInitialized(false);
- }
+ assert(first_frame_state_.count('P') and first_frame_state_.count('O') and incoming_capture_->getFrame() != nullptr);
+
+ sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
+ first_pos_,
+ incoming_capture_->getFrame()->getState().vector("PO"),
+ incoming_feature->getMeasurement());
+ // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_max)
+ if ((first_pos_ - incoming_feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
+ sensor_gnss_->setEnuMapInitialized(false);
}
+ // Fix ENU-MAP if separated enough from first pos ( > enu_map_fix_dist )
+ if ((first_pos_ - incoming_feature->getMeasurement()).norm() > params_gnss_->enu_map_fix_dist)
+ sensor_gnss_->fixEnuMap();
+
// Notify if KF created
if (KF_created)
getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_gnss_->time_tolerance);
@@ -153,7 +155,6 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
{
- // CREATE CONSTRAINT --------------------
//WOLF_DEBUG("creating the factor...");
// 2d
if (getProblem()->getDim() == 2)
@@ -163,83 +164,94 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
return FactorBase::emplace<FactorGnssFix3d>(_ftr, _ftr, sensor_gnss_, shared_from_this(), params_->apply_loss_function, FAC_ACTIVE);
}
-bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac)
-{
- //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
- // Cast feature
- auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
- // copy states
- Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
- Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getO()->getState());
- Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
- Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
- Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
- Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
- Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
- // create double* array of a copy of the state
- double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
- pitch_ENU_map.data(), yaw_ENU_map.data()};
- // create residuals pointer
- Eigen::VectorXd residuals(3);
- // evaluate the factor in this state
- fac->evaluate(parameters, residuals.data(), nullptr);
- // discard if residual too high evaluated at the current estimation
- if (residuals.norm() > 1e3)
- {
- WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
- WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),"\nError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residuals).transpose());
- fac->remove();
- return true;
- }
- return false;
-}
-
bool ProcessorGnssFix::voteForKeyFrame() const
{
//WOLF_DEBUG("voteForKeyFrame...");
// Null lastKF
- if (!last_KF_capture_)
+ if (not last_KF_capture_)
{
WOLF_DEBUG("KF because of null lastKF");
return true;
}
// Depending on time since the last KF with gnssfix capture
- if (!last_KF_capture_ || (incoming_capture_->getTimeStamp() - last_KF_capture_->getTimeStamp()) > params_gnss_->max_time_span)
+ if (not last_KF_capture_ or
+ (incoming_capture_->getTimeStamp() - last_KF_capture_->getTimeStamp()) > params_gnss_->max_time_span)
{
WOLF_DEBUG("KF because of time since last KF");
return true;
}
// ENU not defined
- if (!sensor_gnss_->isEnuDefined())
+ if (not sensor_gnss_->isEnuDefined())
{
WOLF_DEBUG("KF because of enu not defined");
return true;
}
// ENU-MAP not initialized and can be initialized
- if ( sensor_gnss_->isEnuDefined() &&
- !sensor_gnss_->isEnuMapInitialized() &&
- (first_feature_->getMeasurement()-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
+ if (params_gnss_->init_enu_map and
+ not first_frame_state_.empty() and
+ sensor_gnss_->isEnuDefined() and
+ not sensor_gnss_->isEnuMapInitialized() and
+ not sensor_gnss_->isEnuMapFixed() and
+ (first_pos_-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min and
+ (first_pos_-incoming_pos_out_.pos).norm() < params_gnss_->enu_map_init_dist_max)
{
WOLF_DEBUG("KF because of enu map not initialized");
- assert(first_capture_ != nullptr);
return true;
}
// Distance criterion (ENU defined and ENU-MAP initialized)
- if (last_KF_capture_ != nullptr && (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+ if (last_KF_capture_ != nullptr and
+ (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
{
WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
return true;
}
- // TODO: more alternatives?
// otherwise
return false;
}
+bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
+{
+ WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+
+ // Cast feature
+ auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
+
+ // copy states
+ Eigen::VectorXd x(gnss_ftr->getCapture()->getFrame()->getP()->getState());
+ Eigen::VectorXd o(gnss_ftr->getCapture()->getFrame()->getO()->getState());
+ Eigen::VectorXd x_antena(sensor_gnss_->getP()->getState());
+ Eigen::VectorXd t_ENU_map(sensor_gnss_->getEnuMapTranslation()->getState());
+ Eigen::VectorXd roll_ENU_map(sensor_gnss_->getEnuMapRoll()->getState());
+ Eigen::VectorXd pitch_ENU_map(sensor_gnss_->getEnuMapPitch()->getState());
+ Eigen::VectorXd yaw_ENU_map(sensor_gnss_->getEnuMapYaw()->getState());
+
+ // create double* array of a copy of the state
+ double* parameters[7] = {x.data(), o.data(), x_antena.data(), t_ENU_map.data(), roll_ENU_map.data(),
+ pitch_ENU_map.data(), yaw_ENU_map.data()};
+ // residual
+ Eigen::Vector3d residual;
+
+ // evaluate the factor in this state
+ fac->evaluate(parameters, residual.data(), nullptr);
+
+ // evaluate if residual too high at the current estimation
+ if (residual.norm() > params_gnss_->outlier_residual_th)
+ {
+ WOLF_WARN("Discarding GNSS FIX Factor, considered OUTLIER");
+ WOLF_TRACE("Feature: ", fac->getMeasurement().transpose(),
+ "\n\tError: ",(fac->getMeasurementSquareRootInformationUpper().inverse()*residual).transpose(),
+ "\n\tResidual: ",residual.transpose(),
+ "\n\tResidual norm: ",residual.norm(), "(max: ", params_gnss_->outlier_residual_th, ")");
+ return true;
+ }
+ return false;
+}
+
bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 6bbe11a7105a10ce544a4572401be211a4556b6c..3c8bdf9b09497c10d76af3b25f60786853dcec61 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -1,9 +1,12 @@
#include "gnss/processor/processor_tracker_gnss.h"
#include "gnss/capture/capture_gnss.h"
+#include "gnss/feature/feature_gnss_tdcp.h"
#include "gnss/factor/factor_gnss_tdcp.h"
+#include "gnss/factor/factor_gnss_tdcp_3d.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
#include "gnss_utils/utils/rcv_position.h"
+#include "gnss_utils/tdcp.h"
namespace wolf
{
@@ -14,7 +17,8 @@ ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params
outliers_pseudorange_(0),
outliers_tdcp_(0),
inliers_pseudorange_(0),
- inliers_tdcp_(0)
+ inliers_tdcp_(0),
+ first_pos_(Eigen::Vector3d::Zero())
{
}
@@ -24,7 +28,9 @@ void ProcessorTrackerGnss::preProcess()
GnssUtils::SnapshotPtr inc_snapshot = std::static_pointer_cast<CaptureGnss>(incoming_ptr_)->getSnapshot();
- WOLF_INFO("preprocess: initial observations: ", inc_snapshot->getObservations()->size());
+#ifdef _WOLF_DEBUG
+ int n_initial = inc_snapshot->getObservations()->size();
+#endif
// compute satellites positions
if (!inc_snapshot ->satellitesComputed())
@@ -35,21 +41,45 @@ void ProcessorTrackerGnss::preProcess()
* - set ENU
* - compute azimuths and elevations
* - Take the eventual discarded sats by RAIM
+ * - initialize clock bias
**/
fix_incoming_ = GnssUtils::computePos(*inc_snapshot->getObservations(),
*inc_snapshot->getNavigation(),
params_tracker_gnss_->fix_opt);
+ // Initialize clock bias stateblocks in capture
+ if (fix_incoming_.success)
+ {
+ incoming_ptr_->getStateBlock('T') ->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(0)));
+ if (sensor_gnss_->isStateBlockDynamic('G'))
+ incoming_ptr_->getStateBlock('G')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(1)));
+ if (sensor_gnss_->isStateBlockDynamic('E'))
+ incoming_ptr_->getStateBlock('E')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(2)));
+ if (sensor_gnss_->isStateBlockDynamic('M'))
+ incoming_ptr_->getStateBlock('M')->setState(Eigen::Vector1d(CLIGHT*fix_incoming_.rcv_bias(3)));
+
+ if (first_pos_.squaredNorm() < 1e-6)
+ first_pos_ = fix_incoming_.pos;
+ }
+
// Set ECEF-ENU
if (!sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuModeAuto() and fix_incoming_.success)
{
- WOLF_INFO("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
+ WOLF_DEBUG("setting ECEF-ENU: ", fix_incoming_.pos.transpose());
sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
}
+ // Fix ENU-MAP
+ if ((fix_incoming_.pos - first_pos_).norm() > params_tracker_gnss_->enu_map_fix_dist)
+ {
+ sensor_gnss_->getEnuMapTranslation()->fix();
+ sensor_gnss_->getEnuMapRoll()->fix();
+ sensor_gnss_->getEnuMapPitch()->fix();
+ sensor_gnss_->getEnuMapYaw()->fix();
+ }
WOLF_DEBUG("TS: ", incoming_ptr_->getTimeStamp(), " - Fix solution (ECEF): ", fix_incoming_.pos.transpose(), " - Fix solution (GEO): ", fix_incoming_.lat_lon.transpose());
- WOLF_INFO("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size());
+ //WOLF_DEBUG("preprocess: RTKLIB excluded observations: ", fix_incoming_.discarded_sats.size());
// filter observations (available ephemeris, constellations and elevation&SNR)
inc_snapshot->filterObservations(fix_incoming_.discarded_sats, // discarded sats
fix_incoming_.sat_azel,
@@ -57,7 +87,7 @@ void ProcessorTrackerGnss::preProcess()
false, // check carrier phase
params_tracker_gnss_->gnss_opt);
- WOLF_INFO("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
+ //WOLF_DEBUG("preprocess: filtered observations: ", inc_snapshot->getObservations()->size());
// compute corrected Ranges
inc_snapshot->computeRanges(fix_incoming_.sat_azel,
@@ -75,7 +105,11 @@ void ProcessorTrackerGnss::preProcess()
untracked_incoming_features_[feat->satNumber()] = feat;
}
- WOLF_INFO("preprocess: untracked incoming features: ", untracked_incoming_features_.size());
+ WOLF_DEBUG("ProcessorTrackerGnss::preProcess()",
+ "\n\tinitial observations: ", n_initial,
+ "\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(),
+ "\n\tgnssutils discarded: ", n_initial - untracked_incoming_features_.size() - fix_incoming_.discarded_sats.size(),
+ "\n\tdetected incoming features: ", untracked_incoming_features_.size());
}
unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _features_in,
@@ -86,17 +120,19 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
if (_features_in.empty())
return 0;
- WOLF_INFO("tracking " , _features_in.size() , " features...");
+ //WOLF_DEBUG("tracking " , _features_in.size() , " features...");
assert(_capture == incoming_ptr_);
for (auto feat_in : _features_in)
{
- assert(std::dynamic_pointer_cast<FeatureGnssSatellite>(feat_in) != nullptr);
+ auto feat_in_gnss = std::dynamic_pointer_cast<FeatureGnssSatellite>(feat_in);
+ assert(feat_in_gnss);
int sat_num = std::static_pointer_cast<FeatureGnssSatellite>(feat_in)->satNumber();
- if (untracked_incoming_features_.count(sat_num) != 0)
+ if (untracked_incoming_features_.count(sat_num) != 0 and
+ std::abs(untracked_incoming_features_.at(sat_num)->getObservation().L[0]) > 1e-12) // Track only carrier phase valid
{
auto ftr = untracked_incoming_features_[sat_num];
untracked_incoming_features_.erase(sat_num);
@@ -107,28 +143,31 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
}
}
- WOLF_INFO("Tracked features: " , _features_out.size());
+ WOLF_DEBUG("ProcessorTrackerGnss::trackFeatures: tracked " , _features_out.size(), " (of ", _features_in.size(), ")");
return _features_out.size();
}
bool ProcessorTrackerGnss::voteForKeyFrame() const
{
- //WOLF_INFO("ProcessorTrackerGnss::voteForKeyFrame");
+ //WOLF_DEBUG("ProcessorTrackerGnss::voteForKeyFrame");
// too old origin
if (origin_ptr_== nullptr or (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp()) > params_tracker_gnss_->max_time_span )
{
- WOLF_INFO( "Vote for KF because of time span or null origin" );
+ WOLF_DEBUG( "Vote for KF because of time span or null origin" );
return true;
}
- // known features
+ /* KNOWN FEATURES
+ * If tracked sats are lower than minimum (min_features_for_keyframe), a KF in last will be useful
+ * 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() < params_tracker_feature_->min_features_for_keyframe
- and !untracked_last_features_.empty())
+ and not untracked_last_features_.empty() )
{
- WOLF_INFO( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
+ WOLF_DEBUG( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
return true;
}
@@ -151,10 +190,14 @@ unsigned int ProcessorTrackerGnss::detectNewFeatures(const int& _max_new_feature
if (_features_out.size() == _max_new_features)
break;
+ // discard non-valid carrier phase
+ if (std::abs(std::static_pointer_cast<FeatureGnssSatellite>(feat_pair.second)->getObservation().L[0]) < 1e-12)
+ continue;
+
_features_out.push_back(feat_pair.second);
WOLF_DEBUG("feature " , feat_pair.second->id() , " detected!" );
}
- WOLF_DEBUG(_features_out.size() , " features detected!");
+ WOLF_DEBUG(_features_out.size() , " new features detected!");
return _features_out.size();
}
@@ -165,95 +208,79 @@ void ProcessorTrackerGnss::establishFactors()
// initialize frame state with antenna position in map coordinates
// (since we don't have orientation for removing extrinsics)
- if (params_tracker_gnss_->init_frames)
+ if (params_tracker_gnss_->init_frames and fix_last_.success)
last_ptr_->getFrame()->getP()->setState(sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap()));
FactorBasePtrList new_factors;
- bool last_clock_bias_init = false;
-
// PSEUDO RANGE FACTORS (all sats)
- for (auto ftr : last_ptr_->getFeatureList())
- {
- auto ftr_sat = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr);
- if (ftr_sat == nullptr)
- continue;
-
- // Check valid measurement
- if (ftr_sat->getMeasurement()(0) < 1e-12)
+ if (params_tracker_gnss_->pseudo_ranges)
+ for (auto ftr : last_ptr_->getFeatureList())
{
- WOLF_DEBUG("Feature with not valid pseudorange");
- continue;
- }
+ auto ftr_sat = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr);
+ if (ftr_sat == nullptr)
+ continue;
- // Add clock bias stateblocks in capture
- if (last_ptr_->getStateBlock("T") == nullptr)
- {
- last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem());
- last_ptr_->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)), true), getProblem());
- last_ptr_->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)), true), getProblem());
- last_ptr_->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)), true), getProblem());
- //WOLF_INFO("last clock bias set: ", last_ptr_->getStateBlock("T")->getState());
- }
- // Initialize clock bias stateblocks in capture
- else if (!last_clock_bias_init)
- {
- last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
- last_ptr_->getStateBlock("TG")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(1)));
- last_ptr_->getStateBlock("TE")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(2)));
- last_ptr_->getStateBlock("TC")->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(3)));
- //WOLF_INFO("last clock bias initialized: ", last_ptr_->getStateBlock("T")->getState());
- last_clock_bias_init = true;
- }
+ // Check valid measurement
+ if (std::abs(ftr_sat->getMeasurement()(0)) < 1e-12)
+ {
+ WOLF_WARN("Ignoring feature with not valid pseudorange: ", ftr_sat->getMeasurement()(0));
+ continue;
+ }
- // unfix clock bias inter-constellation
- if (ftr_sat->getSatellite().sys == SYS_GLO)
- last_ptr_->getStateBlock("TG")->unfix();
- if (ftr_sat->getSatellite().sys == SYS_GAL)
- last_ptr_->getStateBlock("TE")->unfix();
- if (ftr_sat->getSatellite().sys == SYS_CMP)
- last_ptr_->getStateBlock("TC")->unfix();
-
- // Emplace a pseudo range factor
- auto new_fac = FactorBase::emplace<FactorGnssPseudoRange>(ftr_sat,
- ftr_sat,
- sensor_gnss_,
- shared_from_this(),
- params_->apply_loss_function);
- new_factors.push_back(new_fac);
-
- WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
- }
+ // unfix clock bias
+ last_ptr_->getStateBlock('T')->unfix();
+ // unfix clock bias inter-constellation (if observed)
+ if (ftr_sat->getSatellite().sys == SYS_GLO)
+ last_ptr_->getStateBlock('G')->unfix();
+ if (ftr_sat->getSatellite().sys == SYS_GAL)
+ last_ptr_->getStateBlock('E')->unfix();
+ 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(),
+ params_->apply_loss_function);
+ new_factors.push_back(new_fac);
+
+ WOLF_DEBUG("FactorGnssPseudoRange emplaced for satellite: ", ftr_sat->satNumber());
+ }
// TDCP FACTORS (tracked sats)
if (origin_ptr_ != last_ptr_ and params_tracker_gnss_->gnss_opt.tdcp.enabled)
{
- // iterate over tracked features of last
- for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
+ // FACTOR per pair of KF (FactorGnssTdcp3d)
+ if (params_tracker_gnss_->gnss_opt.tdcp.batch)
{
- // current feature
- auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
- assert(ftr_k != nullptr);
+ WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
- // check valid measurement
- if (ftr_k->getObservation().L[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
- continue;
+ for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
+ KF_rit != getProblem()->getTrajectory()->rend();
+ KF_rit++)
+ {
+ FrameBasePtr KF = (*KF_rit);
- WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
+ WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
- // emplace a tdcp factor from last to each KF
- auto ts_ftr_r_map = track_matrix_.trackAtKeyframes(ftr_k_pair.first);
- for (auto ts_ftr_r_it = ts_ftr_r_map.rbegin(); ts_ftr_r_it != ts_ftr_r_map.rend(); ts_ftr_r_it++)
- {
- // cast reference feature
- auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
- assert(ftr_r != nullptr);
+ // discard non-key frames, last-last pair and frames without CaptureGnss
+ if (not KF->isKey() or
+ KF == last_ptr_->getFrame() or
+ KF->getCaptureOf(getSensor(),"CaptureGnss") == nullptr)
+ continue;
+
+ // static cast
+ auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+ auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
// dt
- double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;
+ double dt = last_ptr_->getTimeStamp() - ref_cap_gnss->getTimeStamp();
+ WOLF_DEBUG("TDCP BATCH dt = ", dt);
- // discard incomming-last and last-last
- if (dt < 0 or ftr_k == ftr_r)
+ // discard strange cases
+ if (dt <= 0)
continue;
// within time window
@@ -261,41 +288,127 @@ void ProcessorTrackerGnss::establishFactors()
break;
// discard removing Frame/capture/feature
- if (ftr_r->isRemoving() or ftr_r->getCapture()->isRemoving() or ftr_r->getCapture()->getFrame()->isRemoving())
+ if (ref_cap_gnss->isRemoving() or ref_cap_gnss->getFrame()->isRemoving())
continue;
- // check valid measurement
- if (ftr_r->getObservation().L[0] < 1e-12) // TODO: move to GnssUtils::isValid(double, Combination)
- continue;
+ // get common sats from tracking
+ auto matches = track_matrix_.matches(ref_cap_gnss, last_cap_gnss);
+ WOLF_DEBUG("TDCP BATCH matches ", matches.size());
+ std::set<int> common_sats;
+ for (auto match : matches)
+ {
+ assert(std::dynamic_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
+ assert(std::dynamic_pointer_cast<FeatureGnssSatellite>(match.second.second)->satNumber());
+ assert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber() ==
+ std::static_pointer_cast<FeatureGnssSatellite>(match.second.second)->satNumber());
+ common_sats.insert(std::static_pointer_cast<FeatureGnssSatellite>(match.second.first)->satNumber());
+ }
+ WOLF_DEBUG("TDCP BATCH common_sats: ", common_sats.size());
+ for (auto sat : common_sats)
+ std::cout << sat << " ";
+ std::cout << std::endl;
+
+ // DEBUG: FIND COMMON SATELLITES OBSERVATIONS
+ std::set<int> common_sats_debug = GnssUtils::Observations::findCommonObservations(*ref_cap_gnss->getSnapshot()->getObservations(),
+ *last_cap_gnss->getSnapshot()->getObservations());
+ WOLF_DEBUG("TDCP BATCH common_sats_debug: ", common_sats_debug.size());
+ for (auto sat : common_sats_debug)
+ std::cout << sat << " ";
+ std::cout << std::endl;
+
+ // compute TDCP batch
+ Eigen::Vector4d d = Eigen::Vector4d::Zero();
+ Eigen::Matrix4d cov_d = Eigen::Matrix4d::Identity();
+ double residual;
+
+ if (GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
+ last_cap_gnss->getSnapshot(),
+ (fix_last_.success ? fix_last_.pos : Eigen::Vector3d::Zero()),
+ common_sats,
+ d,
+ cov_d,
+ residual,
+ std::set<int>(),
+ params_tracker_gnss_->tdcp_batch_params))
+ {
+ WOLF_DEBUG("TDCP BATCH d = ", d.transpose());
+ WOLF_DEBUG("TDCP BATCH cov =\n", cov_d);
- WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
+ // EMPLACE FEATURE
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(d.head<3>()), Eigen::Matrix3d(cov_d.topLeftCorner<3,3>()));
- // Add clock bias stateblock in capture
- if (last_ptr_->getStateBlock("T") == nullptr)
- last_ptr_->addStateBlock("T", std::make_shared<StateBlock>(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)), false), getProblem());
- // Initialize clock bias stateblock in capture
- else if (!last_clock_bias_init)
+ // EMPLACE FACTOR
+ FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
+ }
+ else
{
- last_ptr_->getStateBlock("T") ->setState(Eigen::Vector1d(CLIGHT*fix_last_.rcv_bias(0)));
- last_clock_bias_init = true;
+ WOLF_DEBUG("TDCP BATCH failed");
}
+ }
+ }
+ // FACTOR per SATELLITE (FactorGnssTdcp)
+ else
+ {
+ // iterate over tracked features of last
+ for (auto ftr_k_pair : track_matrix_.snapshot(last_ptr_))
+ {
+ // current feature
+ auto ftr_k = std::dynamic_pointer_cast<FeatureGnssSatellite>(ftr_k_pair.second);
+ assert(ftr_k != nullptr);
+
+ // check valid measurement
+ assert(std::abs(ftr_k->getObservation().L[0]) > 1e-12);
+
+ WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
- // emplace tdcp factor
- double var_tdcp = dt * std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_atm,2) + std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_carrier,2);
- auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
- sqrt(var_tdcp),
- ftr_r,
- ftr_k,
- sensor_gnss_,
- shared_from_this(),
- params_tracker_gnss_->gnss_opt.tdcp.loss_function);
- new_factors.push_back(new_fac);
-
- // WOLF_INFO( "Factor: track: " , feature_in_last->trackId(),
- // " origin: " , feature_in_origin->id() ,
- // " from last: " , feature_in_last->id() );
+ // unfix clock bias
+ last_ptr_->getStateBlock('T')->unfix();
+
+ // emplace a tdcp factor from last to each KF
+ auto ts_ftr_r_map = track_matrix_.trackAtKeyframes(ftr_k_pair.first);
+ for (auto ts_ftr_r_it = ts_ftr_r_map.rbegin(); ts_ftr_r_it != ts_ftr_r_map.rend(); ts_ftr_r_it++)
+ {
+ // cast reference feature
+ auto ftr_r = std::dynamic_pointer_cast<FeatureGnssSatellite>(ts_ftr_r_it->second);
+ assert(ftr_r != nullptr);
+
+ // dt
+ double dt = ftr_k->getCapture()->getTimeStamp() - ts_ftr_r_it->first;
+
+ // discard incomming-last and last-last
+ if (dt < 0 or ftr_k == ftr_r)
+ continue;
+
+ // within time window
+ if (dt > params_tracker_gnss_->gnss_opt.tdcp.time_window)
+ break;
+
+ // discard removing Frame/capture/feature
+ if (ftr_r->isRemoving() or ftr_r->getCapture()->isRemoving() or ftr_r->getCapture()->getFrame()->isRemoving())
+ continue;
+
+ // check valid measurement
+ assert(std::abs(ftr_r->getObservation().L[0]) > 1e-12);
+
+ WOLF_DEBUG("previous feature at KF: ", ftr_r->getCapture()->getFrame()->id(), " sat: ", ftr_r->satNumber(), " id: ", ftr_r->id());
+
+ // emplace tdcp factor
+ double var_tdcp = dt * std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_atm,2) + std::pow(params_tracker_gnss_->gnss_opt.tdcp.sigma_carrier,2);
+ auto new_fac = FactorBase::emplace<FactorGnssTdcp>(ftr_k,
+ sqrt(var_tdcp),
+ ftr_r,
+ ftr_k,
+ sensor_gnss_,
+ shared_from_this(),
+ params_tracker_gnss_->gnss_opt.tdcp.loss_function);
+ new_factors.push_back(new_fac);
+
+ // WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
+ // " origin: " , feature_in_origin->id() ,
+ // " from last: " , feature_in_last->id() );
+ }
+ WOLF_DEBUG("All TDCP factors emplaced!");
}
- WOLF_DEBUG("All TDCP factors emplaced!");
}
}
@@ -335,7 +448,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
FactorBasePtrList remove_fac;
- //WOLF_INFO( "PR ", getName()," rejectOutlier...");
+ //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
// PseudoRange states
Eigen::Vector3d x;
@@ -348,7 +461,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
// state for pseudoranges is fix solution if OK
- if (cap == last_ptr_ and fix_last_.stat != 0)
+ if (cap == last_ptr_ and fix_last_.stat != 0 and params_tracker_gnss_->remove_outliers_with_fix)
{
WOLF_DEBUG("OUTLIERS COMPUTED USING fix_last");
x = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * fix_last_.pos + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
@@ -368,20 +481,20 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
//std::cout << "x_antena_pr: " << x_antena_pr.transpose() << std::endl;
//std::cout << "frame p: " << cap->getFrame()->getP()->getState().transpose() << std::endl;
//std::cout << "frame o: " << cap->getFrame()->getO()->getState().transpose() << std::endl;
- //std::cout << "sb clock: " << cap->getStateBlock("T")->getState() << std::endl;
- //std::cout << "sb clock glo: " << cap->getStateBlock("TG")->getState() << std::endl;
- //std::cout << "sb clock gal: " << cap->getStateBlock("TE")->getState() << std::endl;
- //std::cout << "sb clock cmp: " << cap->getStateBlock("TC")->getState() << std::endl;
+ //std::cout << "sb clock: " << cap->getStateBlock('T')->getState() << std::endl;
+ //std::cout << "sb clock glo: " << cap->getStateBlock('G')->getState() << std::endl;
+ //std::cout << "sb clock gal: " << cap->getStateBlock('E')->getState() << std::endl;
+ //std::cout << "sb clock cmp: " << cap->getStateBlock('M')->getState() << std::endl;
//std::cout << "sb antena: " << sensor_gnss_->getP()->getState().transpose() << std::endl;
}
else
{
x = cap->getFrame()->getP()->getState();
o = cap->getFrame()->getO()->getState();
- clock_bias = cap->getStateBlock("T")->getState();
- clock_bias_glo = cap->getStateBlock("TG")->getState();
- clock_bias_gal = cap->getStateBlock("TE")->getState();
- clock_bias_cmp = cap->getStateBlock("TC")->getState();
+ clock_bias = cap->getStateBlock('T')->getState();
+ clock_bias_glo = cap->getStateBlock('G')->getState();
+ clock_bias_gal = cap->getStateBlock('E')->getState();
+ clock_bias_cmp = cap->getStateBlock('M')->getState();
x_antena_pr = sensor_gnss_->getP()->getState();
}
@@ -390,8 +503,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
Eigen::Vector4d o_k(cap->getFrame()->getO()->getState());
Eigen::Vector3d x_r(cap->getFrame()->getP()->getState());
Eigen::Vector4d o_r(cap->getFrame()->getO()->getState());
- Eigen::Vector1d clock_bias_k(cap->getStateBlock("T")->getState());
- Eigen::Vector1d clock_bias_r(cap->getStateBlock("T")->getState());
+ Eigen::Vector1d clock_bias_k(cap->getStateBlock('T')->getState());
+ Eigen::Vector1d clock_bias_r(cap->getStateBlock('T')->getState());
Eigen::Vector3d x_antena(sensor_gnss_->getP()->getState());
// Common states
@@ -477,8 +590,8 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
// RTKLIB FIX error
//int sat = std::static_pointer_cast<FeatureGnssSatellite>(fac->getFeature())->getSatellite().sat;
//assert(fix_last_.prange_residuals.count(sat) && "sat not used when computing fix!");
- //WOLF_INFO("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
- //WOLF_INFO("RTKLIB pntpos error = ", fix_last_.prange_residuals.at(sat));
+ //WOLF_DEBUG("FactorGnssPseudoRange error = ", fac->getMeasurementSquareRootInformationUpper().inverse()*residual);
+ //WOLF_DEBUG("RTKLIB pntpos error = ", fix_last_.prange_residuals.at(sat));
// discard if residual too high evaluated at the current estimation
if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
@@ -504,7 +617,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
// update ref frame
x_r = fac_tdcp->getCaptureOther()->getFrame()->getP()->getState();
o_r = fac_tdcp->getCaptureOther()->getFrame()->getO()->getState();
- clock_bias_r = fac_tdcp->getCaptureOther()->getStateBlock("T")->getState();
+ clock_bias_r = fac_tdcp->getCaptureOther()->getStateBlock('T')->getState();
parameters_tdcp[0] = x_r.data();
parameters_tdcp[1] = o_r.data();
parameters_tdcp[2] = clock_bias_r.data();
@@ -512,7 +625,7 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
// evaluate
fac_tdcp->evaluate(parameters_tdcp, &residual, nullptr);
- //WOLF_INFO("FactorGnssTdcp with residual = ", residual);
+ //WOLF_DEBUG("FactorGnssTdcp with residual = ", residual);
// discard if residual too high evaluated at the current estimation
if (std::abs(residual) > params_tracker_gnss_->outlier_residual_th)
@@ -538,18 +651,22 @@ void ProcessorTrackerGnss::removeOutliers(FactorBasePtrList fac_list, CaptureBas
//assert(false);
fac->remove();
}
- WOLF_INFO("ProcessorTrackerGnss::removeOutliers:",
- "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)",
- "\n\tTDCP: ", outliers_tdcp_, "\t( ", (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_), " %)",
- "\n\tsats:");
- std::string sat_out_str("");
- for (auto sat_out : sat_outliers_)
- {
- const int& sat = sat_out.first;
- int sys = satsys(sat,NULL);
- const unsigned int& outliers = sat_out.second;
- std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl;
- }
+ WOLF_DEBUG("ProcessorTrackerGnss::removeOutliers:",
+ "\n\tPseudorange: ", outliers_pseudorange_, "\t( ", (100.0 * outliers_pseudorange_)/(outliers_pseudorange_+inliers_pseudorange_), " %)");
+ if (params_tracker_gnss_->gnss_opt.tdcp.enabled and params_tracker_gnss_->remove_outliers_tdcp)
+ std::cout << "\tTDCP: "
+ << outliers_tdcp_
+ << "\t( "
+ << (100.0 * outliers_tdcp_)/(outliers_tdcp_+inliers_tdcp_)
+ << " \%)\n";
+ //std::cout << "\tsats:";
+ //for (auto sat_out : sat_outliers_)
+ //{
+ // const int& sat = sat_out.first;
+ // int sys = satsys(sat,NULL);
+ // const unsigned int& outliers = sat_out.second;
+ // std::cout << "\t\tsat " << sat << "(" << sys << "): " << outliers << std::endl;
+ //}
}
} // namespace wolf
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index f709e3414c836206169fdb07ac1b06510b84d1eb..471384faccb0c9734dca708ef7c9946675c391f3 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -23,17 +23,46 @@ SensorGnss::SensorGnss(const Eigen::VectorXd& _extrinsics,
{
assert(_extrinsics.size() == 3 && "Bad extrinsics size");
- addStateBlock("t", std::make_shared<StateBlock>(3, params_->translation_fixed));
- addStateBlock("r", std::make_shared<StateAngle>(0.0, params_->roll_fixed));
- addStateBlock("p", std::make_shared<StateAngle>(0.0, params_->pitch_fixed));
- addStateBlock("y", std::make_shared<StateAngle>(0.0, params_->yaw_fixed));
-
+ // STATE BLOCKS
+ // ENU-MAP
+ addStateBlock('t', std::make_shared<StateBlock>(3, params_->translation_fixed), false);
+ addStateBlock('r', std::make_shared<StateAngle>(0.0, params_->roll_fixed), false);
+ addStateBlock('p', std::make_shared<StateAngle>(0.0, params_->pitch_fixed), false);
+ addStateBlock('y', std::make_shared<StateAngle>(0.0, params_->yaw_fixed), false);
+ // clock bias
+ addStateBlock(CLOCK_BIAS_KEY, std::make_shared<StateBlock>(1,false), true); // receiver clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GLO_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GLO_dynamic); // GPS-GLO clock bias
+ addStateBlock(CLOCK_BIAS_GPS_GAL_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_GAL_dynamic); // GPS-GAL clock bias
+ addStateBlock(CLOCK_BIAS_GPS_CMP_KEY, std::make_shared<StateBlock>(1,true), params_->clock_bias_GPS_CMP_dynamic); // GPS-CMP clock bias
+
+ // ENU-ECEF
// Mode "manual": ENU provided via params
if (params_->ENU_mode == "manual")
setEcefEnu(params_->ENU_latlonalt, false);
// Mode "ECEF": ENU = ECEF (disabling this coordinates system)
if (params_->ENU_mode == "ECEF")
setEnuEcef(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero());
+
+ // PRIORS
+ // prior to ENU-MAP (avoid non-observable problem)
+ if (params_->ENU_mode != "ECEF")
+ {
+ if (not params_->translation_fixed)
+ addPriorParameter('t', Eigen::Vector3d::Zero(), 10*Eigen::Matrix3d::Identity());
+ if (not params_->roll_fixed)
+ addPriorParameter('r', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ if (not params_->pitch_fixed)
+ addPriorParameter('p', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ if (not params_->yaw_fixed)
+ addPriorParameter('y', Eigen::Vector1d::Zero(), M_PI*Eigen::Matrix1d::Identity());
+ }
+ // prior to inter-constellation clock bias (avoid non-observable problem)
+ if (not params_->clock_bias_GPS_GLO_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_GLO_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+ if (not params_->clock_bias_GPS_GAL_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_GAL_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
+ if (not params_->clock_bias_GPS_CMP_dynamic)
+ addPriorParameter(CLOCK_BIAS_GPS_CMP_KEY, Eigen::Vector1d::Zero(), Eigen::Matrix1d::Identity());
}
SensorGnss::~SensorGnss()
@@ -77,6 +106,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
// 2d
if (getProblem()->getDim() == 2)
{
+ WOLF_INFO("SensorGnss: initializing ENU-MAP 2D case....");
+
// compute antena vector (from 1 to 2) in MAP
Eigen::Vector2d t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
Eigen::Vector2d t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<double>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
@@ -135,7 +166,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXd& _pose_MAP_frameENU, con
else
{
//TODO
- std::runtime_error("not implemented yet");
+ WOLF_WARN("initialization of ENU-MAP not implemented in 3D")
+ //throw std::runtime_error("not implemented yet");
}
}
diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp
index 013354b41f320c099f1de1218c14b22f8aee2fcd..401cd95f9b323146483990de071886a8c9be728d 100644
--- a/test/gtest_factor_gnss_fix_2d.cpp
+++ b/test/gtest_factor_gnss_fix_2d.cpp
@@ -5,10 +5,10 @@
* \author: jvallve
*/
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/processor/processor_gnss_fix.h"
#include "gnss/capture/capture_gnss_fix.h"
@@ -50,23 +50,24 @@ void fixAllStates(SensorGnssPtr gnss_sensor_ptr, FrameBasePtr frame_ptr)
frame_ptr->getO()->fix();
}
-void computeParamSizes(const CeresManagerPtr& ceres_mgr_ptr, int& num_params_reduced, int& num_param_blocks_reduced )
+void computeParamSizes(const SolverCeresPtr& solver_ceres, int& num_params_reduced, int& num_param_blocks_reduced )
{
num_param_blocks_reduced = 0;
num_params_reduced = 0;
std::vector<double*> param_blocks;
- ceres_mgr_ptr->getCeresProblem()->GetParameterBlocks(¶m_blocks);
+ solver_ceres->getCeresProblem()->GetParameterBlocks(¶m_blocks);
for (auto pb : param_blocks)
{
std::vector<ceres::ResidualBlockId> residual_blocks;
- ceres_mgr_ptr->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
+ solver_ceres->getCeresProblem()->GetResidualBlocksForParameterBlock(pb,&residual_blocks);
- if (!ceres_mgr_ptr->getCeresProblem()->IsParameterBlockConstant(pb) && !residual_blocks.empty())
+ if (not solver_ceres->getCeresProblem()->IsParameterBlockConstant(pb) and
+ not residual_blocks.empty())
{
- num_param_blocks_reduced ++;
- num_params_reduced += ceres_mgr_ptr->getCeresProblem()->ParameterBlockLocalSize(pb);
+ num_param_blocks_reduced ++;
+ num_params_reduced += solver_ceres->getCeresProblem()->ParameterBlockLocalSize(pb);
}
}
}
@@ -87,7 +88,7 @@ Vector3d t_ecef_antena = R_ecef_enu * (R_enu_map * (R_map_base * t_base_antena +
// WOLF
ProblemPtr problem_ptr = Problem::create("PO", 2);
-CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
+SolverCeresPtr solver_ceres = std::make_shared<SolverCeres>(problem_ptr);
SensorGnssPtr gnss_sensor_ptr = std::static_pointer_cast<SensorGnss>(problem_ptr->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frame_ptr;
@@ -95,7 +96,7 @@ FrameBasePtr frame_ptr;
TEST(FactorGnssFix2dTest, configure_tree)
{
- ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
+ solver_ceres->getSolverOptions().max_num_iterations = 100;
// Configure sensor and processor
gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
@@ -120,8 +121,8 @@ TEST(FactorGnssFix2dTest, configure_tree)
gnss_sensor_ptr->process(cap_gnss_ptr);
// Checks
- ASSERT_TRUE(problem_ptr->check(1));
- ASSERT_TRUE(frame_ptr->isKey());
+ EXPECT_TRUE(problem_ptr->check(1));
+ EXPECT_TRUE(frame_ptr->isKey());
}
/*
@@ -146,28 +147,28 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
frame_ptr->setState(frm_dist, "PO", {2,1});
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 2);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 2);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::FULL);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 2);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 2);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(frame_ptr->getState().at("P"), t_map_base.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(frame_ptr->getState().at('P'), t_map_base.head(2), 1e-6);
}
/*
@@ -191,26 +192,26 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_orientation)
frame_ptr->getO()->setState(frm_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 1);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 1);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
+ EXPECT_MATRIX_APPROX(frame_ptr->getO()->getState(), o_map_base, 1e-6);
}
/*
@@ -234,26 +235,29 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_yaw)
gnss_sensor_ptr->getEnuMapYaw()->setState(o_enu_map_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
+ EXPECT_TRUE(solver_ceres->check());
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 1);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 1);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::FULL);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapYaw()->getState(), o_enu_map, 1e-6);
+
+ problem_ptr->print(4,1,1,1);
}
/*
@@ -278,26 +282,26 @@ TEST(FactorGnssFix2dTest, gnss_1_enu_map_position)
gnss_sensor_ptr->getEnuMapTranslation()->setState(t_enu_map_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 3);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 3);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getEnuMapTranslation()->getState().head(2), t_enu_map.head(2), 1e-6);
}
/*
@@ -322,26 +326,26 @@ TEST(FactorGnssFix2dTest, gnss_1_base_antena)
gnss_sensor_ptr->getP()->setState(base_antena_dist);
// --------------------------- update solver
- ceres_mgr_ptr->update();
+ solver_ceres->update();
// --------------------------- check problem parameters
int num_params_reduced, num_param_blocks_reduced;
- computeParamSizes(ceres_mgr_ptr, num_params_reduced, num_param_blocks_reduced);
+ computeParamSizes(solver_ceres, num_params_reduced, num_param_blocks_reduced);
- ASSERT_EQ(num_param_blocks_reduced, 1);
- ASSERT_EQ(num_params_reduced, 3);
+ EXPECT_EQ(num_param_blocks_reduced, 1);
+ EXPECT_EQ(num_params_reduced, 3);
// --------------------------- solve
- std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::QUIET);
+ std::string report = solver_ceres->solve(SolverManager::ReportVerbosity::QUIET);
// --------------------------- check summary parameters & residuals
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameter_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_parameters_reduced, 3);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residual_blocks_reduced, 1);
- ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameter_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_parameters_reduced, 3);
+ EXPECT_EQ(solver_ceres->getSummary().num_residual_blocks_reduced, 1);
+ EXPECT_EQ(solver_ceres->getSummary().num_residuals_reduced, 3);
// --------------------------- check solver solution
- ASSERT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
+ EXPECT_MATRIX_APPROX(gnss_sensor_ptr->getP()->getState().head(2), t_base_antena.head(2), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp
index b98aba78a545046425735fe44b0d4562eade2741..1f2ae8a11753e24140707f7dcebe849b73949405 100644
--- a/test/gtest_factor_gnss_pseudo_range.cpp
+++ b/test/gtest_factor_gnss_pseudo_range.cpp
@@ -1,7 +1,7 @@
+#include <core/ceres_wrapper/solver_ceres.h>
#include <core/utils/utils_gtest.h>
#include "core/problem/problem.h"
-#include "core/ceres_wrapper/ceres_manager.h"
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/capture/capture_gnss.h"
#include "gnss/factor/factor_gnss_pseudo_range.h"
@@ -28,7 +28,7 @@ GnssUtils::Range range1, range2, range3, range4;
// WOLF
ProblemPtr prb = Problem::create("PO", 3);
-CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
+SolverCeresPtr solver = std::make_shared<SolverCeres>(prb);
SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frm;
CaptureGnssPtr cap;
@@ -108,10 +108,8 @@ void setUpProblem()
// capture
cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr);
- cap->addStateBlock("T", std::make_shared<StateBlock>(clock_drift, false), prb);
- cap->addStateBlock("TG", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
- cap->addStateBlock("TE", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
- cap->addStateBlock("TC", std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true), prb);
+ cap->getStateBlock('T')->unfix();
+ cap->getStateBlock('T')->setState(clock_drift);
// features
ftr1 = FeatureBase::emplace<FeatureGnssSatellite>(cap, obsd_t(), sat1, range1); // obsd_t data is not used in pseudo range factors
@@ -137,7 +135,7 @@ void setUpProblem()
ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3);
ASSERT_MATRIX_APPROX(frm->getO()->getState(), q_map_base.coeffs(), 1e-3);
// clock drift
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
}
void fixAllStates()
@@ -166,10 +164,10 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
// fix/unfix
fixAllStates();
- cap->getStateBlock("T")->unfix();
+ cap->getStateBlock('T')->unfix();
// perturb
- cap->getStateBlock("T")->perturb(1e2);
+ cap->getStateBlock('T')->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
@@ -180,7 +178,7 @@ TEST(FactorGnssPreusoRangeTest, observe_clock_drift)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
}
}
@@ -221,11 +219,11 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
// fix/unfix
fixAllStates();
frm->getP()->unfix();
- cap->getStateBlock("T")->unfix();
+ cap->getStateBlock('T')->unfix();
// perturb
frm->getP()->perturb(1);
- cap->getStateBlock("T")->perturb(1e2);
+ cap->getStateBlock('T')->perturb(1e2);
// all 4 factors
@@ -234,7 +232,7 @@ TEST(FactorGnssPreusoRangeTest, observe_frame_p_clock)
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm->getP()->getState(), t_map_base, 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift, cap->getStateBlock('T')->getState(), 1e-3);
}
}
diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp
index f0d45e936c2ea7f36d92f3951952a1051c9f9932..cd51ec56a04bfb97bb3ba42e6a3372b1ed4f0d9c 100644
--- a/test/gtest_factor_gnss_tdcp.cpp
+++ b/test/gtest_factor_gnss_tdcp.cpp
@@ -1,3 +1,5 @@
+#include <core/ceres_wrapper/solver_ceres.h>
+
#include "gnss/factor/factor_gnss_tdcp.h"
#include <core/utils/utils_gtest.h>
@@ -5,7 +7,6 @@
#include "gnss/sensor/sensor_gnss.h"
#include "gnss/capture/capture_gnss.h"
-#include "core/ceres_wrapper/ceres_manager.h"
using namespace Eigen;
using namespace wolf;
@@ -32,7 +33,7 @@ GnssUtils::Range range1_r, range2_r, range3_r, range4_r, range1_k, range2_k, ran
// WOLF
ProblemPtr prb = Problem::create("PO", 3);
-CeresManagerPtr solver = std::make_shared<CeresManager>(prb);
+SolverCeresPtr solver = std::make_shared<SolverCeres>(prb);
SensorGnssPtr gnss_sensor = std::static_pointer_cast<SensorGnss>(prb->installSensor("SensorGnss", "gnss", t_base_antena, std::make_shared<ParamsSensorGnss>()));
FrameBasePtr frm_r, frm_k;
CaptureGnssPtr cap_r, cap_k;
@@ -128,11 +129,13 @@ void setUpProblem()
// capture r
cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr);
- cap_r->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_r, false), prb);
+ cap_r->getStateBlock('T')->unfix();
+ cap_r->getStateBlock('T')->setState(clock_drift_r);
// capture k
cap_k = CaptureBase::emplace<CaptureGnss>(frm_k, TimeStamp(1), gnss_sensor, nullptr);
- cap_k->addStateBlock("T", std::make_shared<StateBlock>(clock_drift_k, false), prb);
+ cap_k->getStateBlock('T')->unfix();
+ cap_k->getStateBlock('T')->setState(clock_drift_k);
// features r
ftr1_r = FeatureBase::emplace<FeatureGnssSatellite>(cap_r, obsd_t(), sat1_r, range1_r);
@@ -167,8 +170,8 @@ void setUpProblem()
ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
ASSERT_MATRIX_APPROX(frm_k->getO()->getState(), q_map_base_k.coeffs(), 1e-3);
// clock drift
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
}
void fixAllStates()
@@ -199,10 +202,10 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_r)
// fix/unfix
fixAllStates();
- cap_r->getStateBlock("T")->unfix();
+ cap_r->getStateBlock('T')->unfix();
// perturb
- cap_r->getStateBlock("T")->perturb(1e2);
+ cap_r->getStateBlock('T')->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
@@ -213,7 +216,7 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_r)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
}
}
@@ -227,10 +230,10 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_k)
// fix/unfix
fixAllStates();
- cap_k->getStateBlock("T")->unfix();
+ cap_k->getStateBlock('T')->unfix();
// perturb
- cap_k->getStateBlock("T")->perturb(1e2);
+ cap_k->getStateBlock('T')->perturb(1e2);
// Only 1 factor
fac2->setStatus(FAC_INACTIVE);
@@ -241,7 +244,7 @@ TEST(FactorGnssTdcpTest, observe_clock_drift_k)
std::string report = solver->solve(SolverManager::ReportVerbosity::FULL);
//std::cout << report << std::endl;
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
}
}
@@ -308,11 +311,11 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
// fix/unfix
fixAllStates();
frm_k->getP()->unfix();
- cap_k->getStateBlock("T")->unfix();
+ cap_k->getStateBlock('T')->unfix();
// perturb
frm_k->getP()->perturb(1);
- cap_k->getStateBlock("T")->perturb(1e2);
+ cap_k->getStateBlock('T')->perturb(1e2);
// all 4 factors
@@ -321,7 +324,7 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_k)
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm_k->getP()->getState(), t_map_base_k, 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_k, cap_k->getStateBlock('T')->getState(), 1e-3);
}
}
@@ -336,11 +339,11 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
// fix/unfix
fixAllStates();
frm_r->getP()->unfix();
- cap_r->getStateBlock("T")->unfix();
+ cap_r->getStateBlock('T')->unfix();
// perturb
frm_r->getP()->perturb(1);
- cap_r->getStateBlock("T")->perturb(1e2);
+ cap_r->getStateBlock('T')->perturb(1e2);
// all 4 factors
@@ -349,7 +352,7 @@ TEST(FactorGnssTdcpTest, observe_frame_p_clock_r)
//std::cout << report << std::endl;
ASSERT_MATRIX_APPROX(frm_r->getP()->getState(), t_map_base_r, 1e-3);
- ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock("T")->getState(), 1e-3);
+ ASSERT_MATRIX_APPROX(clock_drift_r, cap_r->getStateBlock('T')->getState(), 1e-3);
}
}