diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index 00d71c8ea1e5ffae633ab84e190e22ff8fa2c8c7..dd8ba5aeb905616f7f2a768ed4b0d9d842da542a 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -130,6 +130,11 @@ inline const Eigen::Vector3s& SensorGnss::gettEnuEcef() const
return t_ENU_ECEF_;
}
+inline Eigen::Vector3s SensorGnss::gettEnuMap() const
+{
+ return getStateBlockPtrStatic(3)->getState();
+}
+
template<typename T>
inline Eigen::Matrix<T,3,3> SensorGnss::computeREnuMap(const T& _r,const T& _p,const T& _y) const
{
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index b4efaea1fc846095dc71af477a1164005aa42105..89d42df2a49051cc205a494db2334c948c3b204b 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -38,63 +38,63 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
new_frame = KF_pack->key_frame;
}
// MAKE KF
- else if (voteForKeyFrame() && permittedKeyFrame())
+ else if (permittedKeyFrame() && voteForKeyFrame())
{
new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp());
getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_gnss_->time_tolerance);
WOLF_DEBUG( "PR ", getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp());
}
- // ESTABLISH FACTOR
- if (new_frame)
- {
- // LINK CAPTURE
- incoming_capture_->link(new_frame); // Add incoming Capture to the new Frame
-
- // EMPLACE FEATURES
- WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
- auto ftr = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
-
- // EMPLACE FACTOR
- new_fac = emplaceFactor(ftr);
-
- // outlier rejection
- if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
- if (rejectOutlier(new_fac))
- new_fac = nullptr;
-
- // store last KF
- if (new_fac)
- last_KF_= new_frame;
- }
-
- // SET ECEF_ENU IF:
- // 1 - not initialized
- // 2 - factor established
- if (!sensor_gnss_->isEnuDefined() && new_fac != nullptr)
- {
- WOLF_DEBUG("setting ecef enu");
- sensor_gnss_->setEcefEnu(incoming_capture_->getData());
+ // nothing else if no frame created
+ if (new_frame == nullptr)
+ return;
- // Store the first capture that established a factor
- first_capture_ = incoming_capture_;
- }
-
- // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
- // 1 - ENU-ECEF defined
- // 2 - not initialized
- // 3 - current capture in key-frame with factor
- // 4 - two factors established (first and current) in frames separated enough ( > enu_map_init_dist_min)
- if ( sensor_gnss_->isEnuDefined() &&
- !sensor_gnss_->isEnuMapInitialized() &&
- new_fac != nullptr &&
- first_capture_ != nullptr && first_capture_->getFrame() != nullptr && incoming_capture_->getFrame() != nullptr &&
- (first_capture_->getFrame()->getState()-incoming_capture_->getFrame()->getState()).norm() > params_gnss_->enu_map_init_dist_min)
- {
- WOLF_DEBUG("initializing enu map");
- sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState(), first_capture_->getData(),
- incoming_capture_->getFrame()->getState(), incoming_capture_->getData());
- }
+ // ESTABLISH FACTOR
+ // link capture
+ incoming_capture_->link(new_frame);
+
+ // emplace features
+ WOLF_DEBUG( "PR ", getName()," - emplacing the feature...");
+ auto ftr = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_capture_->getData(),incoming_capture_->getDataCovariance());
+
+ // emplace factor
+ new_fac = emplaceFactor(ftr);
+
+ // outlier rejection (only can be evaluated if ENU defined and ENU-MAP initialized)
+ if (sensor_gnss_->isEnuDefined() && sensor_gnss_->isEnuMapInitialized())
+ if (rejectOutlier(new_fac))
+ {
+ new_fac->remove();
+ return;
+ }
+
+ // store last KF
+ last_KF_= new_frame;
+
+ // Define ENU (if not defined)
+ if (!sensor_gnss_->isEnuDefined())
+ {
+ WOLF_DEBUG("setting ecef enu");
+ sensor_gnss_->setEcefEnu(incoming_capture_->getData());
+
+ }
+ // Store the first capture that established a factor (for initializing ENU-MAP)
+ if (first_capture_ == nullptr)
+ first_capture_ = incoming_capture_;
+
+ // Initialize ENU-MAP if some NECESSARY conditions:
+ // 1 - ENU defined
+ // 2 - ENU-MAP not initialized
+ // 3 - two factors established (first and current) separated enough ( > enu_map_init_dist_min)
+ assert(first_capture_->getFrame() != nullptr && incoming_capture_->getFrame() != nullptr);
+ if ( sensor_gnss_->isEnuDefined() &&
+ !sensor_gnss_->isEnuMapInitialized() &&
+ (first_capture_->getData()-incoming_capture_->getData()).norm() > params_gnss_->enu_map_init_dist_min)
+ {
+ WOLF_DEBUG("initializing enu map");
+ sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState(), first_capture_->getData(),
+ incoming_capture_->getFrame()->getState(), incoming_capture_->getData());
+ }
}
FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr)
@@ -145,8 +145,24 @@ bool ProcessorGnssFix::voteForKeyFrame()
if (!last_KF_ || (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
return true;
- // Distance criterion
- if ((incoming_capture_->getFrame()->getP()->getState() - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
+ // ENU not defined
+ if (!sensor_gnss_->isEnuDefined())
+ return true;
+ // ENU-MAP not initialized
+ if (!sensor_gnss_->isEnuMapInitialized())
+ {
+ assert(first_capture_ != nullptr);
+ // if distance enough to initialize: make KF
+ if ((first_capture_->getData()-incoming_capture_->getData()).norm() > params_gnss_->enu_map_init_dist_min)
+ return true;
+ else
+ return false;
+ }
+
+ // Distance criterion (ENU defined and ENU-MAP initialized)
+ // convert fix data from ECEF to MAP reference frame
+ Eigen::Vector3s incoming_fix_map = sensor_gnss_->getREnuMap().transpose() * (sensor_gnss_->getREnuEcef() * incoming_capture_->getData() + sensor_gnss_->gettEnuEcef() - sensor_gnss_->gettEnuMap());
+ if ((incoming_fix_map.head(2) - last_KF_->getP()->getState()).norm() > params_gnss_->dist_traveled)
return true;
// TODO: more alternatives?
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 2dd0a9179de1eb44b7726e6dd43702949b5bfd95..c1e0e65eadef6d9ae9dee21ade9d54a3bea3fe2d 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -106,21 +106,21 @@ bool ProcessorGnssSingleDiff::voteForKeyFrame()
if (last_KF_==nullptr)
return true;
- std::cout << "params_gnss_->time_th" << params_gnss_->time_th << std::endl;
- std::cout << "(last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp())" << (last_KF_->getTimeStamp() - incoming_capture_->getTimeStamp()) << std::endl;
+ //std::cout << "params_gnss_->time_th" << params_gnss_->time_th << std::endl;
+ //std::cout << "(incoming_capture_->getTimeStamp() - incoming_capture_->getTimeStamp())" << (incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) << std::endl;
// Depending on time since the last KF with gnssfix capture
if ((incoming_capture_->getTimeStamp() - last_KF_->getTimeStamp()) > params_gnss_->time_th)
return true;
// Distance criterion
- std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
- Eigen::Vector2s v_current_origin = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
- std::cout << "v_current_origin: " << v_current_origin.transpose() << std::endl;
- Eigen::Vector2s v_origin_last_KF = last_KF_->getP()->getState() - incoming_capture_->getOriginFrame()->getP()->getState();
- std::cout << "v_origin_last_KF: " << v_origin_last_KF.transpose() << std::endl;
- std::cout << "v_current_origin + v_origin_last_KF: " << (v_current_origin + v_origin_last_KF).transpose() << std::endl;
- if ((v_current_origin + v_origin_last_KF).norm() > params_gnss_->dist_traveled)
+ Eigen::Vector2s v_origin_current = (sensor_gnss_->getREnuMap().transpose() * sensor_gnss_->getREnuEcef() * incoming_capture_->getData()).head<2>();
+ Eigen::Vector2s v_lastKF_origin = incoming_capture_->getOriginFrame()->getP()->getState() - last_KF_->getP()->getState();
+ //std::cout << "params_gnss_->dist_traveled" << params_gnss_->dist_traveled << std::endl;
+ //std::cout << "v_origin_current: " << v_origin_current.transpose() << std::endl;
+ //std::cout << "v_lastKF_origin: " << v_lastKF_origin.transpose() << std::endl;
+ //std::cout << "v_lastKF_origin + v_origin_current: " << (v_lastKF_origin + v_origin_current).transpose() << std::endl;
+ if ((v_lastKF_origin + v_origin_current).norm() > params_gnss_->dist_traveled)
return true;
// TODO: more alternatives?