ProcessorGnssFix stores captures and join kfs

include/gnss/processor/processor_gnss_fix.h

@@ -23,8 +23,8 @@
 #define WOLF_PROCESSOR_GNSS_FIX_H
 
 // Wolf includes
+#include "gnss/internal/config.h"
 #include "core/processor/processor_base.h"
-#include "gnss/capture/capture_gnss.h"
 #include "gnss/sensor/sensor_gnss.h"
 
 // Std includes
@@ -127,9 +127,8 @@ class ProcessorGnssFix : public ProcessorBase
         SensorGnssPtr sensor_gnss_;
         ParamsProcessorGnssFixPtr params_gnss_;
         CaptureBasePtr last_KF_capture_, incoming_capture_;
-        FeatureGnssFixPtr last_KF_feature_;
+        FeatureBasePtr last_KF_feature_, incoming_feature_;
         FrameBasePtr last_KF_;
-        GnssUtils::ComputePosOutput incoming_pos_out_;
         Eigen::Vector3d first_pos_;
         VectorComposite first_frame_state_;
 
@@ -155,43 +154,42 @@ class ProcessorGnssFix : public ProcessorBase
          */
         void processKeyFrame(FrameBasePtr _keyframe_ptr) override {};
 
+
         /** \brief trigger in capture
          *
          * Returns true if processCapture() should be called after the provided capture arrived.
          */
-        bool triggerInCapture(CaptureBasePtr) const override;
+        bool triggerInCapture(CaptureBasePtr) const override {return true;};
 
         /** \brief trigger in key-frame
          *
          * The ProcessorTracker only processes incoming captures, then it returns false.
          */
-        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;}
+        bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr) const override {return false;};
 
         /** \brief store key frame
         *
         * Returns true if the key frame should be stored
         */
-        bool storeKeyFrame(FrameBasePtr) override;
+        bool storeKeyFrame(FrameBasePtr) override {return true;};
 
         /** \brief store capture
         *
         * Returns true if the capture should be stored
         */
-        bool storeCapture(CaptureBasePtr) override;
+        bool storeCapture(CaptureBasePtr) override {return false;};
 
         bool voteForKeyFrame() const override;
 
     private:
+        void processStoredFrames();
+        void handleEnuMap(FeatureBasePtr incoming_feature);
+        FeatureBasePtr emplaceFeature(CaptureBasePtr _capture);
         FactorBasePtr emplaceFactor(FeatureBasePtr _ftr_ptr);
         bool detectOutlier(FactorBasePtr ctr_ptr);
 
 };
 
-inline bool ProcessorGnssFix::triggerInCapture(CaptureBasePtr) const
-{
-    return true;
-}
-
 } // namespace wolf
 
 #endif //WOLF_PROCESSOR_GNSS_FIX_H

src/processor/processor_gnss_fix.cpp

@@ -19,12 +19,12 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 //--------LICENSE_END--------
+#include "gnss/processor/processor_gnss_fix.h"
 #include "gnss/capture/capture_gnss.h"
 #include "gnss/capture/capture_gnss_fix.h"
 #include "gnss/factor/factor_gnss_fix_2d.h"
 #include "gnss/factor/factor_gnss_fix_3d.h"
 #include "gnss/feature/feature_gnss_fix.h"
-#include "gnss/processor/processor_gnss_fix.h"
 
 namespace wolf
 {
@@ -41,59 +41,148 @@ ProcessorGnssFix::~ProcessorGnssFix()
     //
 }
 
+void ProcessorGnssFix::processStoredFrames()
+{
+    // clean buffers (useless entries)
+    if (not buffer_frame_.empty() and not buffer_capture_.empty())
+    {
+        buffer_frame_  .removeUpToLower(buffer_capture_.getContainer().begin()->first-params_->time_tolerance);
+        buffer_capture_.removeUpToLower(buffer_frame_  .getContainer().begin()->first-params_->time_tolerance);
+    }
+
+    // iterate over stored frames
+    auto frm_it = buffer_frame_.getContainer().begin();
+    while (frm_it != buffer_frame_.getContainer().end())
+    {
+        // Search for any stored capture within time tolerance of frame
+        auto capture = buffer_capture_.select(frm_it->first, params_->time_tolerance);
+
+        // Capture found
+        if (capture)
+        {
+            // safety check: capture stored already linked
+            if (capture->getFrame())
+            {
+                WOLF_WARN("ProcessorGnssFix::processStoredFrames: any stored CaptureGnss was already linked to a frame");
+                buffer_capture_.getContainer().erase(capture->getTimeStamp());
+                frm_it++;
+                continue;
+            }
+            // link capture
+            capture->link(frm_it->second);
+
+            // get feature
+            FeatureGnssFixPtr feat_fix;
+            auto feature_list = capture->getFeatureList();
+            for (auto feat : feature_list)
+            {
+                feat_fix = std::dynamic_pointer_cast<FeatureGnssFix>(feat);
+                if (feat_fix)
+                    break;
+            }
+            if (not feat_fix)
+            {
+                WOLF_WARN("ProcessorGnssFix::processStoredFrames: A stored CaptureGnss doesn't have any FrameGnssFix");
+                buffer_capture_.getContainer().erase(capture->getTimeStamp());
+                frm_it++;
+                continue;
+            }
+
+            // emplace factor
+            auto fac = emplaceFactor(feat_fix);
+
+            // outlier rejection (only can be evaluated if ENU defined and ENU-MAP well initialized: fixed)
+            if (params_gnss_->remove_outliers and detectOutlier(fac))
+            {
+                feat_fix->remove();
+                buffer_capture_.getContainer().erase(capture->getTimeStamp());
+                frm_it++;
+                continue;
+            }
+
+            // Handle ENU definition, ENU-MAP initialization etc.
+            handleEnuMap(feat_fix);
+
+            // store last KF (if more recent)
+            if (not last_KF_capture_ or last_KF_capture_->getTimeStamp() < capture->getTimeStamp())
+            {
+                last_KF_capture_ = capture;
+                last_KF_feature_ = feat_fix;
+            }
+
+            // remove capture from buffer
+            buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+            // remove frame from buffer
+            frm_it = buffer_frame_.getContainer().erase(frm_it);
+        }
+        else
+            frm_it++;
+    }
+}
+
+FeatureBasePtr ProcessorGnssFix::emplaceFeature(CaptureBasePtr _capture)
+{
+    GnssUtils::ComputePosOutput pos_output;
+    if (std::dynamic_pointer_cast<CaptureGnss>(_capture))
+    {
+        WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming raw measurement. Type:", _capture->getType());
+        auto raw_capture = std::static_pointer_cast<CaptureGnss>(_capture);
+        pos_output = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
+        if (!pos_output.success) // error
+        {
+            WOLF_DEBUG("computePos failed with msg: ", pos_output.msg);
+            return nullptr;
+        }
+    }
+    else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture))
+    {
+        WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", _capture->getType());
+        auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(_capture);
+        pos_output.time = fix_capture->getGpsTimeStamp().getSeconds();           // time_t time;
+        pos_output.sec =  1e-9 * fix_capture->getGpsTimeStamp().getSeconds();    // double sec;
+        pos_output.pos = fix_capture->getPositionEcef();                         // Eigen::Vector3d pos;        // position (m)
+        pos_output.vel = Eigen::Vector3d::Zero();                                // Eigen::Vector3d vel;        // velocity (m/s)
+        pos_output.pos_covar = fix_capture->getCovarianceEcef();                 // Eigen::Matrix3d pos_covar;  // position covariance (m^2)
+        pos_output.type = 0;                                                     // int type;                   // coordinates used (0:xyz-ecef,1:enu-baseline)
+    }
+    else
+        throw std::runtime_error("ProcessorGnssFix::emplaceFeature wrong capture type");
+
+    return FeatureBase::emplace<FeatureGnssFix>(_capture, pos_output);
+}
+
 void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
 {
-    // TODO: keep captures in a buffer and deal with KFpacks
     WOLF_DEBUG("PR ", getName()," process() capture type: ", _capture->getType());
-    incoming_capture_ = _capture;
 
-    bool KF_created = false;
+    // Check capture type
+    if (not std::dynamic_pointer_cast<CaptureGnss>(_capture) and not std::dynamic_pointer_cast<CaptureGnssFix>(_capture))
+        throw std::runtime_error("ProcessorGnssFix::processCapture capture of bad type. Accepted types: CaptureGnss and CaptureGnssFix");
 
-    // Check type of capture: GNSS raw
-    bool israw = false;
-    if (std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr)
-        israw = true;
-    // if not raw nor Fix, not processable
-    else if (std::dynamic_pointer_cast<CaptureGnssFix>(_capture)==nullptr)
-        return;
+    // First process stored frames
+    processStoredFrames();
 
-    // Only process raw if fix_from_raw
-    if (israw and !params_gnss_->fix_from_raw)
+    // Only process raw if fix_from_raw and fix if not fix_from_raw
+    bool israw = std::dynamic_pointer_cast<CaptureGnss>(_capture)!=nullptr;
+    if (israw and not params_gnss_->fix_from_raw)
         return;
-
-    // Only process fix if not fix_from_raw
-    if (!israw and params_gnss_->fix_from_raw)
+    if (not israw and params_gnss_->fix_from_raw)
         return;
 
+    // Process incoming capture
+    incoming_capture_ = _capture;
+    bool KF_created = false;
     FrameBasePtr new_frame = nullptr;
     FactorBasePtr new_fac = nullptr;
 
-    // emplace features
-    if (israw)
-    {
-        auto raw_capture = std::static_pointer_cast<CaptureGnss>(incoming_capture_);
-        incoming_pos_out_ = GnssUtils::computePos(*(raw_capture->getObservations()), *(raw_capture->getNavigation()), params_gnss_->compute_pos_opt);
-        if (!incoming_pos_out_.success) // error
-        {
-            WOLF_DEBUG("computePos failed with msg: ", incoming_pos_out_.msg);
-            return;
-        }
-    }
-    else
-    {
-        WOLF_DEBUG("ProcessorGnssFix: creating feature from incoming navsatfix. Type:", incoming_capture_->getType());
-        auto fix_capture = std::static_pointer_cast<CaptureGnssFix>(incoming_capture_);
-        incoming_pos_out_.time = fix_capture->getGpsTimeStamp().getSeconds();           // time_t time;
-        incoming_pos_out_.sec =  1e-9 * fix_capture->getGpsTimeStamp().getSeconds();    // double sec;
-        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_.type = 0;                                                     // int type;                   // coordinates used (0:xyz-ecef,1:enu-baseline)
-    }
-    auto incoming_feature = FeatureBase::emplace<FeatureGnssFix>(incoming_capture_, incoming_pos_out_);
+    // emplace feature
+    incoming_feature_ = emplaceFeature(incoming_capture_);
+    if (not incoming_feature_)
+        return;
 
     // ALREADY CREATED KF
-    FrameBasePtr keyframe = buffer_frame_.select( incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
+    FrameBasePtr keyframe = buffer_frame_.select(incoming_capture_->getTimeStamp(), params_gnss_->time_tolerance);
     if (keyframe and last_KF_capture_ and keyframe == last_KF_capture_->getFrame())
         keyframe = nullptr;
     if (keyframe)
@@ -108,70 +197,82 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture)
         new_frame = getProblem()->emplaceFrame(incoming_capture_->getTimeStamp());
         KF_created = true;
     }
-    // OTHERWISE return
+    // OTHERWISE store capture
     else
+    {
+        buffer_capture_.emplace(_capture->getTimeStamp(), _capture);
         return;
+    }
 
-    // ESTABLISH FACTOR
     // link capture
     incoming_capture_->link(new_frame);
+
     // emplace factor
-    new_fac = emplaceFactor(incoming_feature);
+    new_fac = emplaceFactor(incoming_feature_);
 
     // 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))
+    if (params_gnss_->remove_outliers and detectOutlier(new_fac))
     {
-        new_frame->remove();
+        if (KF_created)
+            new_frame->remove();
+        else
+            incoming_feature_->remove();
         return;
     }
 
+    // Handle ENU definition, ENU-MAP initialization etc.
+    handleEnuMap(incoming_feature_);
+
     // store last KF
     last_KF_capture_ = incoming_capture_;
-    last_KF_feature_ = incoming_feature;
+    last_KF_feature_ = incoming_feature_;
+
+    // Notify if KF created
+    if (KF_created)
+        getProblem()->keyFrameCallback(new_frame, shared_from_this());
+}
 
+void ProcessorGnssFix::handleEnuMap(FeatureBasePtr feature)
+{
     // Define ENU (if not defined)
     if (!sensor_gnss_->isEnuDefined())
     {
-        WOLF_DEBUG("Defining ecef enu");
-        sensor_gnss_->setEcefEnu(incoming_feature->getMeasurement());
+        WOLF_INFO("Defining ecef enu with first fix");
+        sensor_gnss_->setEcefEnu(feature->getMeasurement());
     }
 
-    // Store the first capture that established a factor (for initializing ENU-MAP)
+    // Store the first capture that established a factor (for later initialization ENU-MAP)
     if (first_frame_state_.empty() and
         not sensor_gnss_->isEnuMapFixed())
     {
-        first_frame_state_ = incoming_capture_->getFrame()->getState();
-        first_pos_ = incoming_feature->getMeasurement();
+        first_frame_state_ = feature->getCapture()->getFrame()->getState();
+        first_pos_ = feature->getMeasurement();
     }
+
     // 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)
+        (first_pos_-feature->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min)
     {
-        assert(first_frame_state_.count('P') and first_frame_state_.count('O') and incoming_capture_->getFrame() != nullptr);
+        assert(first_frame_state_.count('P') and 
+               first_frame_state_.count('O') and
+               feature->getCapture()->getFrame() != nullptr);
 
         sensor_gnss_->initializeEnuMap(first_frame_state_.vector("PO"),
                                        first_pos_,
-                                       incoming_capture_->getFrame()->getState().vector("PO"),
-                                       incoming_feature->getMeasurement());
+                                       feature->getCapture()->getFrame()->getState().vector("PO"),
+                                       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)
+        if ((first_pos_ - 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)
+    if ((first_pos_ - 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());
 }
 
 FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr _ftr)
@@ -216,8 +317,8 @@ bool ProcessorGnssFix::voteForKeyFrame() const
         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)
+        (first_pos_-incoming_feature_->getMeasurement()).norm() > params_gnss_->enu_map_init_dist_min and
+        (first_pos_-incoming_feature_->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_max)
     {
         WOLF_DEBUG("KF because of enu map not initialized");
         return true;
@@ -225,9 +326,9 @@ bool ProcessorGnssFix::voteForKeyFrame() const
 
     // Distance criterion (ENU defined and ENU-MAP initialized)
     if (last_KF_capture_ != nullptr and
-        (incoming_pos_out_.pos - last_KF_feature_->getMeasurement()).norm() > params_gnss_->dist_traveled)
+        (incoming_feature_->getMeasurement() - 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());
+        WOLF_DEBUG("KF because of distance criterion: ", (incoming_feature_->getMeasurement() - last_KF_feature_->getMeasurement()).norm());
         return true;
     }
 
@@ -239,6 +340,9 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
 {
     WOLF_DEBUG( "PR ", getName()," rejectOutlier...");
 
+    if (not sensor_gnss_->isEnuDefined() or not sensor_gnss_->isEnuMapFixed())
+        return false;
+
     // Cast feature
     auto gnss_ftr = std::static_pointer_cast<FeatureGnssFix>(fac->getFeature());
 
@@ -273,14 +377,6 @@ bool ProcessorGnssFix::detectOutlier(FactorBasePtr fac)
     return false;
 }
 
-bool ProcessorGnssFix::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
-    return true;
-}
-bool ProcessorGnssFix::storeCapture(CaptureBasePtr _cap_ptr)
-{
-    return false;
-}
 void ProcessorGnssFix::configure(SensorBasePtr _sensor)
 {
     sensor_gnss_ = std::static_pointer_cast<SensorGnss>(_sensor);

test/gtest_factor_gnss_fix_2d.cpp

@@ -153,6 +153,8 @@ TEST(FactorGnssFix2dTest, configure_tree)
  */
 TEST(FactorGnssFix2dTest, gnss_1_map_base_position)
 {
+    problem_ptr->print();
+    
     // --------------------------- Reset states
     resetStates(gnss_sensor_ptr, frame_ptr, o_enu_map, t_base_antena, t_enu_map, t_map_base, o_map_base);