Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/plugins/gnss
1 result
Show changes
Commits on Source (18)
Expand all Hide whitespace changes
Inline Side-by-side
Showing
with 783 additions and 407 deletions
include/gnss/factor/factor_gnss_pseudo_range.h
View file @ 29ef3ecf
......@@ -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(),
......
include/gnss/factor/factor_gnss_tdcp.h
View file @ 29ef3ecf
......@@ -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");
......
include/gnss/factor/factor_gnss_tdcp_2d.h
View file @ 29ef3ecf
......@@ -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
View file @ 29ef3ecf
......@@ -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
View file @ 29ef3ecf
#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
View file @ 29ef3ecf
......@@ -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_gnss_fix.h
View file @ 29ef3ecf
......@@ -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);
};
......
include/gnss/processor/processor_tracker_gnss.h
View file @ 29ef3ecf
......@@ -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
......
include/gnss/sensor/sensor_gnss.h
View file @ 29ef3ecf
......@@ -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_
src/capture/capture_gnss.cpp
View file @ 29ef3ecf
......@@ -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()
......
src/processor/processor_gnss_fix.cpp
View file @ 29ef3ecf
......@@ -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()->emplaceFrame(KEY, 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()->emplaceFrame(KEY, 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;
......
src/processor/processor_tracker_gnss.cpp
View file @ 29ef3ecf
This diff is collapsed.
src/sensor/sensor_gnss.cpp
View file @ 29ef3ecf
......@@ -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");
}
}
......
test/gtest_factor_gnss_fix_2d.cpp
View file @ 29ef3ecf
......@@ -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(&param_blocks);
solver_ceres->getCeresProblem()->GetParameterBlocks(&param_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)
......
test/gtest_factor_gnss_pseudo_range.cpp
View file @ 29ef3ecf
#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);
}
}
......
test/gtest_factor_gnss_tdcp.cpp
View file @ 29ef3ecf
#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);
}
}
......