fixed some bugs

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
 {

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?

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?