added ENU-MAP fix and isFixed and work in proc_gnss_fix

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);

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_

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");

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");
     }
 }