From ab9030e88efc53a7fa3eeae5418e6d2e7cac746d Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Sat, 20 Jun 2020 12:37:55 +0200
Subject: [PATCH] added ENU-MAP fix and isFixed and work in proc_gnss_fix
---
include/gnss/processor/processor_gnss_fix.h | 18 ++-
include/gnss/sensor/sensor_gnss.h | 18 +++
src/processor/processor_gnss_fix.cpp | 117 ++++++++++----------
src/sensor/sensor_gnss.cpp | 5 +-
4 files changed, 91 insertions(+), 67 deletions(-)
diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index d4f9b8ac0..d9fa9f1db 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -22,8 +22,8 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
GnssUtils::Options compute_pos_opt;
double max_time_span;
double dist_traveled;
- double enu_map_init_dist_min;
- double enu_map_fix_time;
+ double enu_map_init_dist_min, enu_map_init_dist_max;
+ double enu_map_fix_dist;
double outlier_residual_th;
ParamsProcessorGnssFix() = default;
@@ -34,8 +34,11 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
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_fix_time = _server.getParam<double> (prefix + _unique_name + "/enu_map_fix_time");
+ 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");
@@ -72,7 +75,7 @@ struct ParamsProcessorGnssFix : public ParamsProcessorBase
"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_time: " + std::to_string(enu_map_fix_time) + "\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" +
@@ -102,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);
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 459482316..ee379c90f 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -103,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);
@@ -114,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>
@@ -220,6 +222,22 @@ inline void SensorGnss::setEnuMapRotationInitialized(const bool& _init)
R_ENU_MAP_initialized_ = _init;
}
+inline bool SensorGnss::isEnuMapFixed() const
+{
+ return getEnuMapTranslation()->isFixed() and
+ getEnuMapRoll()->isFixed() and
+ getEnuMapPitch()->isFixed() and
+ getEnuMapYaw()->isFixed();
+}
+
+inline void SensorGnss::fixEnuMap()
+{
+ getEnuMapTranslation()->fix();
+ getEnuMapRoll()->fix();
+ getEnuMapPitch()->fix();
+ getEnuMapYaw()->fix();
+}
+
} // namespace wolf
#endif //SENSOR_GPS_H_
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 86cbd6063..f4d7fe283 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -10,8 +10,7 @@ namespace wolf
ProcessorGnssFix::ProcessorGnssFix(ParamsProcessorGnssFixPtr _params_gnss) :
ProcessorBase("ProcessorGnssFix", 0, _params_gnss),
- params_gnss_(_params_gnss),
- first_capture_(nullptr)
+ params_gnss_(_params_gnss)
{
//
}
@@ -68,14 +67,7 @@ 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_);
@@ -98,23 +90,22 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
// OTHERWISE return
else
return;
- assert(new_frame);
// 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 (params_gnss_->remove_outliers and sensor_gnss_->isEnuDefined() and sensor_gnss_->isEnuMapInitialized())
- if (detectOutlier(new_fac))
- {
- new_frame->remove();
- return;
- }
-
- // link capture
- incoming_capture_->link(new_frame);
+ // 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_;
@@ -128,35 +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 (params_gnss_->init_enu_map and !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);
- // Fix ENU-MAP
- if (incoming_capture_->getTimeStamp() - first_capture_->getTimeStamp() > params_gnss_->enu_map_fix_time)
- {
- sensor_gnss_->getEnuMapTranslation()->fix();
- sensor_gnss_->getEnuMapRoll()->fix();
- sensor_gnss_->getEnuMapPitch()->fix();
- sensor_gnss_->getEnuMapYaw()->fix();
+ 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);
@@ -164,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)
@@ -178,45 +168,47 @@ 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_INFO("KF because of enu not defined");
+ WOLF_DEBUG("KF because of enu not defined");
return true;
}
// ENU-MAP not initialized and can be initialized
if (params_gnss_->init_enu_map and
+ not first_frame_state_.empty() and
sensor_gnss_->isEnuDefined() and
- !sensor_gnss_->isEnuMapInitialized() and
- !first_capture_->isRemoving() and
- (first_feature_->getMeasurement()-incoming_pos_out_.pos).norm() > params_gnss_->enu_map_init_dist_min)
+ 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_INFO("KF because of enu map not initialized");
- assert(first_capture_ != nullptr);
+ WOLF_DEBUG("KF because of enu map not initialized");
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_INFO("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
+ WOLF_DEBUG("KF because of distance criterion: ", (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm());
return true;
}
- // TODO: more alternatives?
// otherwise
return false;
@@ -224,9 +216,11 @@ bool ProcessorGnssFix::voteForKeyFrame() const
bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
{
- //WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
+ 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());
@@ -235,14 +229,17 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
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
+ // residual
Eigen::Vector3d residual;
+
// evaluate the factor in this state
fac->evaluate(parameters, residual.data(), nullptr);
- // discard if residual too high evaluated at the current estimation
+
+ // 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");
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 9a6d6aae2..c48add446 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -84,6 +84,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>();
@@ -142,7 +144,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");
}
}
--
GitLab