Skip to content
Snippets Groups Projects
Commit 61663225 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

wip

parent f8ca50a0
No related branches found
No related tags found
2 merge requests!28release after RAL,!27After 2nd RAL submission
Hide whitespace changes
Inline Side-by-side
include/gnss/factor/factor_gnss_tdcp_2d.h
+ 2
− 2
View file @ 61663225
......@@ -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>
......
include/gnss/factor/factor_gnss_tdcp_3d.h
+ 18
− 18
View file @ 61663225
......@@ -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>
......
include/gnss/factor/factor_gnss_tdcp_batch.h 0 → 100644
+ 120
− 0
View file @ 61663225
#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
include/gnss/feature/feature_gnss_tdcp.h
+ 3
− 0
View file @ 61663225
......@@ -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{};
};
......
include/gnss/processor/processor_tracker_gnss.h
+ 21
− 6
View file @ 61663225
......@@ -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
......
src/processor/processor_tracker_gnss.cpp
+ 187
− 84
View file @ 61663225
#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!");
}
}
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment