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_tracker_gnss.h b/include/gnss/processor/processor_tracker_gnss.h
index aa3a19a7d0cb1b15ef9d3e5cd18379d19f956a8c..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,11 +17,12 @@ 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, remove_outliers_with_fix;
double outlier_residual_th;
- bool init_frames;
- double enu_map_fix_time;
+ bool init_frames, pseudo_ranges;
+ double enu_map_fix_dist;
ParamsProcessorTrackerGnss() = default;
ParamsProcessorTrackerGnss(std::string _unique_name, const ParamsServer& _server):
@@ -31,14 +33,15 @@ struct ParamsProcessorTrackerGnss : public ParamsProcessorTrackerFeature
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_time = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_time");
+ 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
@@ -56,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");
@@ -63,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)
@@ -76,7 +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_time: " + std::to_string(enu_map_fix_time) + "\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"
@@ -94,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"
@@ -128,7 +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_;
- TimeStamp first_ts_;
+ 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/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 47a8d4a5e77bcb773a4e42edcf7e420395df85f3..209bc3a446db43628acae75d27556cbb2ac32df7 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
{
@@ -15,7 +18,7 @@ ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params
outliers_tdcp_(0),
inliers_pseudorange_(0),
inliers_tdcp_(0),
- first_ts_() //invalid timestamp
+ first_pos_(Eigen::Vector3d::Zero())
{
}
@@ -52,6 +55,9 @@ void ProcessorTrackerGnss::preProcess()
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
@@ -61,7 +67,7 @@ void ProcessorTrackerGnss::preProcess()
sensor_gnss_->setEcefEnu(fix_incoming_.pos, true);
}
// Fix ENU-MAP
- if (incoming_ptr_->getTimeStamp() - first_ts_ > params_tracker_gnss_->enu_map_fix_time)
+ if ((fix_incoming_.pos - first_pos_).norm() > params_tracker_gnss_->enu_map_fix_dist)
{
sensor_gnss_->getEnuMapTranslation()->fix();
sensor_gnss_->getEnuMapRoll()->fix();
@@ -97,10 +103,6 @@ void ProcessorTrackerGnss::preProcess()
untracked_incoming_features_[feat->satNumber()] = feat;
}
- // store as first timestamp (if any not-filtered satellite)
- if (!untracked_incoming_features_.empty() and !first_ts_.ok())
- first_ts_ = incoming_ptr_->getTimeStamp();
-
WOLF_INFO("ProcessorTrackerGnss::preProcess()",
"\n\tinitial observations: ", n_initial,
"\n\tRTKLIB discarded: ", fix_incoming_.discarded_sats.size(),
@@ -122,11 +124,13 @@ unsigned int ProcessorTrackerGnss::trackFeatures(const FeatureBasePtrList& _feat
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);
@@ -158,7 +162,7 @@ bool ProcessorTrackerGnss::voteForKeyFrame() const
if (known_features_incoming_.size() < params_tracker_feature_->min_features_for_keyframe
and !untracked_last_features_.empty())
{
- WOLF_DEBUG( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
+ WOLF_INFO( "Vote for KF because of too less known_features_incoming and not empty untracked in last" );
return true;
}
@@ -181,10 +185,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_INFO(_features_out.size() , " new features detected!");
return _features_out.size();
}
@@ -195,78 +203,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;
// 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;
- // 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());
- }
+ // 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
+ 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_INFO("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()->getFrameList().rbegin();
+ KF_rit != getProblem()->getTrajectory()->getFrameList().rend();
+ KF_rit++)
+ {
+ FrameBasePtr KF = (*KF_rit);
- WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->id());
+ WOLF_INFO("TDCP BATCH ref frame ", KF->id());
- // unfix clock bias
- last_ptr_->getStateBlock('T')->unfix();
+ // 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;
- // 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);
+ // 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_INFO("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
@@ -274,31 +283,125 @@ 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_INFO("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_INFO("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_INFO("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_INFO("TDCP BATCH d = ", d.transpose());
+ WOLF_INFO("TDCP BATCH cov =\n", cov_d);
+
+ // EMPLACE FEATURE
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(d.head<3>()), Eigen::Matrix3d(cov_d.topLeftCorner<3,3>()));
+
+ // EMPLACE FACTOR
+ FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
+ }
+ else
+ WOLF_INFO("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);
- 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_INFO( "Factor: track: " , feature_in_last->trackId(),
- // " origin: " , feature_in_origin->id() ,
- // " from last: " , feature_in_last->id() );
+ // check valid measurement
+ assert(std::abs(ftr_k->getObservation().L[0]) > 1e-12);
+
+ WOLF_DEBUG("FeatureGnssSatellite sat: ", ftr_k->satNumber(), " id: ", ftr_k->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_INFO( "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!");
}
}