From c9fc28f96618656b14951c1bb418896fcef7698f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 11 Jul 2019 17:35:47 +0200
Subject: [PATCH] working!

---
 include/gnss/processor/processor_gnss_fix.h   |   7 +-
 .../processor/processor_gnss_single_diff.h    |  27 ++++-
 include/gnss/sensor/sensor_gnss.h             |   6 +-
 src/processor/processor_gnss_fix.cpp          |  54 ++++-----
 src/processor/processor_gnss_single_diff.cpp  |  49 ++++++--
 src/sensor/sensor_gnss.cpp                    | 112 ++++++++++++++++--
 test/gtest_factor_gnss_fix_2D.cpp             |   6 +-
 7 files changed, 206 insertions(+), 55 deletions(-)

diff --git a/include/gnss/processor/processor_gnss_fix.h b/include/gnss/processor/processor_gnss_fix.h
index f2f1d4a51..a32fb08fd 100644
--- a/include/gnss/processor/processor_gnss_fix.h
+++ b/include/gnss/processor/processor_gnss_fix.h
@@ -18,16 +18,19 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssFix);
 struct ProcessorParamsGnssFix : public ProcessorParamsBase
 {
     Scalar time_th;
+    Scalar enu_map_init_dist_min;
     ProcessorParamsGnssFix() = default;
     ProcessorParamsGnssFix(std::string _unique_name, const paramsServer& _server):
         ProcessorParamsBase(_unique_name, _server)
     {
-        time_th = _server.getParam<Scalar>(_unique_name + "/time_th");
+        time_th                 = _server.getParam<Scalar>(_unique_name + "/time_th");
+        enu_map_init_dist_min   = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
     }
     std::string print()
     {
         return "\n" + ProcessorParamsBase::print()
             + "time_th: " + std::to_string(time_th) + "\n";
+            + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
     }
 };
 
@@ -36,7 +39,7 @@ class ProcessorGnssFix : public ProcessorBase
 {
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
-        ProcessorParamsGnssFixPtr params_;
+        ProcessorParamsGnssFixPtr params_gnss_;
         CaptureGnssFixPtr first_capture_ptr_, last_capture_ptr_;
         FrameBasePtr last_gnss_fix_KF_;
 
diff --git a/include/gnss/processor/processor_gnss_single_diff.h b/include/gnss/processor/processor_gnss_single_diff.h
index 708b5c280..41a6ef8a2 100644
--- a/include/gnss/processor/processor_gnss_single_diff.h
+++ b/include/gnss/processor/processor_gnss_single_diff.h
@@ -12,17 +12,38 @@
 namespace wolf {
     
 WOLF_PTR_TYPEDEFS(ProcessorGnssSingleDiff);
+WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsGnssSingleDiff);
+
+
+struct ProcessorParamsGnssSingleDiff : public ProcessorParamsBase
+{
+    Scalar time_th;
+    Scalar enu_map_init_dist_min;
+    ProcessorParamsGnssSingleDiff() = default;
+    ProcessorParamsGnssSingleDiff(std::string _unique_name, const paramsServer& _server):
+        ProcessorParamsBase(_unique_name, _server)
+    {
+        time_th                 = _server.getParam<Scalar>(_unique_name + "/time_th");
+        enu_map_init_dist_min   = _server.getParam<Scalar>(_unique_name + "/enu_map_init_dist_min");
+    }
+    std::string print()
+    {
+        return "\n" + ProcessorParamsBase::print()
+            + "time_th: " + std::to_string(time_th) + "\n";
+            + "enu_map_init_dist_min: " + std::to_string(enu_map_init_dist_min) + "\n";
+    }
+};
     
 //class
 class ProcessorGnssSingleDiff : public ProcessorBase
 {
     protected:
         SensorGnssPtr sensor_gnss_ptr_;
-        ProcessorParamsBasePtr params_;
-        CaptureGnssSingleDiffPtr last_capture_ptr_;
+        ProcessorParamsGnssSingleDiffPtr params_gnss_;
+        CaptureGnssSingleDiffPtr last_capture_ptr_, incoming_capture_ptr_;
 
     public:
-        ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr);
+        ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr);
         virtual ~ProcessorGnssSingleDiff();
         virtual void configure(SensorBasePtr _sensor);
 
diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index db9301ce1..e7204f6fa 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -44,13 +44,14 @@ struct IntrinsicsGnss : public IntrinsicsBase
 class SensorGnss : public SensorBase
 {
     protected:
+        IntrinsicsGnssPtr params_;
         bool ENU_defined_, ENU_MAP_initialized_;
         Eigen::Matrix3s R_ENU_ECEF_;
         Eigen::Vector3s t_ENU_ECEF_;
 
     public:
-        SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr);
-        SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr);
+        SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params);
+        SensorGnss(StateBlockPtr _p_ptr, StateBlockPtr _bias_ptr, IntrinsicsGnssPtr params, StateBlockPtr _t_ENU_MAP_ptr, StateBlockPtr _roll_ENU_MAP_ptr, StateBlockPtr _pitch_ENU_MAP_ptr, StateBlockPtr _yaw_ENU_MAP_ptr);
 
         virtual ~SensorGnss();
 
@@ -68,6 +69,7 @@ class SensorGnss : public SensorBase
         StateBlockPtr getEnuMapYawState() const;
         void initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, const Eigen::Vector3s& _t_ECEF_antenaENU,
                               const Eigen::VectorXs& _pose_MAP_frame2, const Eigen::Vector3s& _t_ECEF_antena2);
+        void setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP);
 
         // Gets
         const Eigen::Matrix3s& getREnuEcef() const;
diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp
index 68becb54e..a12da6a37 100644
--- a/src/processor/processor_gnss_fix.cpp
+++ b/src/processor/processor_gnss_fix.cpp
@@ -7,10 +7,10 @@
 namespace wolf
 {
 
-ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params, SensorGnssPtr _sensor_gnss_ptr) :
-        ProcessorBase("GNSS FIX", _params),
+ProcessorGnssFix::ProcessorGnssFix(ProcessorParamsGnssFixPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) :
+        ProcessorBase("GNSS FIX", _params_gnss),
         sensor_gnss_ptr_(_sensor_gnss_ptr),
-        params_(_params),
+        params_gnss_(_params_gnss),
         first_capture_ptr_(nullptr)
 {
     //
@@ -34,24 +34,24 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
     //std::cout << "WOLF: gnss fix in ENU coordinates: " << p_enu.transpose() << std::endl;
 
     FrameBasePtr new_frame_ptr = nullptr;
-    bool new_fac_created = false;
+    FactorBasePtr new_fac = nullptr;
 
     // ALREADY CREATED KF
-    PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance);
+    PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_gnss_->time_tolerance);
     if (KF_pack && KF_pack->key_frame != last_gnss_fix_KF_)
     {
+        WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
         new_frame_ptr = KF_pack->key_frame;
-        //WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
     }
     // MAKE KF
     else if (voteForKeyFrame() && permittedKeyFrame())
     {
         new_frame_ptr = getProblem()->emplaceFrame(KEY, _capture_ptr->getTimeStamp());
-        getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_->time_tolerance);
-        //WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp());
+        getProblem()->keyFrameCallback(new_frame_ptr, shared_from_this(), params_gnss_->time_tolerance);
+        WOLF_DEBUG( "PR ", getName()," - capture ", _capture_ptr->id(), " appended to new KF " , new_frame_ptr->id() , " TS: ", new_frame_ptr->getTimeStamp());
     }
 
-    // ESTABLISH CONSTRAINT
+    // ESTABLISH FACTOR
     if (new_frame_ptr)
     {
         // LINK CAPTURE
@@ -62,7 +62,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
         auto ftr_ptr = FeatureBase::emplace<FeatureGnssFix>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
 
         // EMPLACE FACTOR
-        auto new_fac = emplaceFactor(ftr_ptr);
+        new_fac = emplaceFactor(ftr_ptr);
 
         // outlier rejection
         if (sensor_gnss_ptr_->isEnuDefined() && sensor_gnss_ptr_->isEnuMapInitialized())
@@ -72,34 +72,34 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture_ptr)
         // store last KF
         if (new_fac)
             last_gnss_fix_KF_= new_frame_ptr;
-
-        //WOLF_DEBUG("Factor established");
     }
 
     // INITIALIZE ENU IF:
     //      1 - not initialized
     //      2 - factor established
-    if (!sensor_gnss_ptr_->isEnuDefined() && new_fac_created)
+    if (!sensor_gnss_ptr_->isEnuDefined() && new_fac != nullptr)
     {
         sensor_gnss_ptr_->setEcefEnu(last_capture_ptr_->getData());
 
-        WOLF_INFO("ProcessorGnssFix: ECEF-ENU initialized.")
+        // Store the first capture that established a factor
+        first_capture_ptr_ = last_capture_ptr_;
     }
 
-    // INITIALIZE ENU_MAP IF 2 NECESSARY CONDITIONS:
-    //      1 - not initialized
-    //      2 - two factors established (first and current)
-    if (!sensor_gnss_ptr_->isEnuMapInitialized() && first_capture_ptr_ && new_fac_created)
+    // 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_ptr_->isEnuDefined() &&
+        !sensor_gnss_ptr_->isEnuMapInitialized() &&
+         new_fac != nullptr &&
+         first_capture_ptr_->getFrame() != nullptr && last_capture_ptr_->getFrame() != nullptr &&
+         (first_capture_ptr_->getFrame()->getState()-last_capture_ptr_->getFrame()->getState()).norm() > params_gnss_->enu_map_init_dist_min)
     {
+        std::cout << "initializing enu map\n";
         sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
                                            last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
-
-        WOLF_INFO("ProcessorGnssFix: ENU-MAP initialized.")
     }
-
-    // Store the first capture that established a factor
-    if (new_fac_created && first_capture_ptr_ == nullptr)
-        first_capture_ptr_ = last_capture_ptr_;
 }
 
 FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
@@ -108,10 +108,10 @@ FactorBasePtr ProcessorGnssFix::emplaceFactor(FeatureBasePtr& ftr_ptr)
     //WOLF_DEBUG("creating the factor...");
     // 2D
     if (getProblem()->getDim() == 2)
-        return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+        return FactorBase::emplace<FactorGnssFix2D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE);
     // 3D
     else
-        return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false);
+        return FactorBase::emplace<FactorGnssFix3D>(ftr_ptr, ftr_ptr, sensor_gnss_ptr_, shared_from_this(), false, FAC_ACTIVE);
 }
 
 bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
@@ -147,7 +147,7 @@ bool ProcessorGnssFix::rejectOutlier(FactorBasePtr fac_ptr)
 bool ProcessorGnssFix::voteForKeyFrame()
 {
     // Depending on time since the last KF with gnssfix capture
-    if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_->time_th)
+    if (!last_gnss_fix_KF_ || (last_capture_ptr_->getTimeStamp() - last_gnss_fix_KF_->getTimeStamp()) > params_gnss_->time_th)
         return true;
 
     // TODO: more alternatives?
diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp
index 42b246c6c..c069eeb81 100644
--- a/src/processor/processor_gnss_single_diff.cpp
+++ b/src/processor/processor_gnss_single_diff.cpp
@@ -6,10 +6,10 @@
 namespace wolf
 {
 
-ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsBasePtr _params, SensorGnssPtr _sensor_gnss_ptr) :
-        ProcessorBase("GNSS SINGLE DIFFERENCES", _params),
+ProcessorGnssSingleDiff::ProcessorGnssSingleDiff(ProcessorParamsGnssSingleDiffPtr _params_gnss, SensorGnssPtr _sensor_gnss_ptr) :
+        ProcessorBase("GNSS SINGLE DIFFERENCES", _params_gnss),
         sensor_gnss_ptr_(_sensor_gnss_ptr),
-        params_(_params)
+        params_gnss_(_params_gnss)
 {
     //
 }
@@ -24,13 +24,17 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
     // TODO: keep captures in a buffer and deal with KFpacks
 
     //WOLF_DEBUG("ProcessorGnssSingleDiff::process()");
-    last_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr);
+    incoming_capture_ptr_ = std::static_pointer_cast<CaptureGnssSingleDiff>(_capture_ptr);
+
+    // discard capture with null or non-key origin frame
+    if (incoming_capture_ptr_->getOriginFrame() == nullptr || incoming_capture_ptr_->getOriginFrame()->isKey())
+        return;
 
     FrameBasePtr new_frame_ptr = nullptr;
 
     // ALREADY CREATED KF
     PackKeyFramePtr KF_pack = buffer_pack_kf_.selectPack( _capture_ptr, params_->time_tolerance);
-    if (KF_pack && KF_pack->key_frame != last_capture_ptr_->getOriginFrame())
+    if (KF_pack && KF_pack->key_frame != incoming_capture_ptr_->getOriginFrame())
     {
         new_frame_ptr = KF_pack->key_frame;
         WOLF_DEBUG( "PR ",getName()," - capture ", _capture_ptr->id(), " appended to existing KF " , KF_pack->key_frame->id() , " TS: ", KF_pack->key_frame->getTimeStamp());
@@ -51,7 +55,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
 
         // EMPLACE FEATURE
         //WOLF_DEBUG("emplacing feature...");
-        auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(last_capture_ptr_, last_capture_ptr_->getData(),last_capture_ptr_->getDataCovariance());
+        auto ftr_ptr = FeatureBase::emplace<FeatureGnssSingleDiff>(incoming_capture_ptr_, incoming_capture_ptr_->getData(),incoming_capture_ptr_->getDataCovariance());
 
         // ADD CONSTRAINT
         //WOLF_DEBUG("emplacing factor...");
@@ -59,6 +63,25 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture_ptr)
 
         //WOLF_DEBUG("Factor established");
     }
+
+    // INITIALIZE ENU_MAP IF 4 NECESSARY CONDITIONS:
+    //      1 - ENU-ECEF defined
+    //      2 - not initialized
+    //      3 - current capture in key-frame with factor
+    //      4 - frames constained by the factor separated enough ( > enu_map_init_dist_min)
+    if ( sensor_gnss_ptr_->isEnuDefined() &&
+        !sensor_gnss_ptr_->isEnuMapInitialized() &&
+         new_fac != nullptr &&
+         incoming_capture_ptr_->getFrame() != nullptr && incoming_capture_ptr_->getFrame()->isKey() &&
+         incoming_capture_ptr_->getData().norm() > params_gnss_->enu_map_init_dist_min)
+    {
+        std::cout << "initializing enu map\n";
+        sensor_gnss_ptr_->initializeEnuMap(first_capture_ptr_->getFrame()->getState(), first_capture_ptr_->getData(),
+                                           last_capture_ptr_->getFrame()->getState(), last_capture_ptr_->getData());
+    }
+
+    last_capture_ptr_ = incoming_capture_ptr_;
+    incoming_capture_ptr_ = nullptr;
 }
 
 FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
@@ -67,7 +90,7 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
     //WOLF_DEBUG("creating the factor...");
     // 2D
     if (getProblem()->getDim() == 2)
-        return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, last_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this());
+        return FactorBase::emplace<FactorGnssSingleDiff2D>(ftr_ptr, ftr_ptr, incoming_capture_ptr_->getOriginFrame(), sensor_gnss_ptr_, shared_from_this());
     // 3D TODO
     else
         std::runtime_error("Single Differences in 3D not implemented yet.");
@@ -77,8 +100,14 @@ FactorBasePtr ProcessorGnssSingleDiff::emplaceFactor(FeatureBasePtr& ftr_ptr)
 
 bool ProcessorGnssSingleDiff::voteForKeyFrame()
 {
-    // TODO
-    return true;
+    // Depending on time since the last KF with gnssfix capture
+    if (last_capture_ptr_==nullptr || (last_capture_ptr_->getTimeStamp() - incoming_capture_ptr_->getTimeStamp()) > params_gnss_->time_th)
+        return true;
+
+    // TODO: more alternatives?
+
+    // otherwise
+    return false;
 }
 
 void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
@@ -88,7 +117,7 @@ void ProcessorGnssSingleDiff::configure(SensorBasePtr _sensor)
 
 ProcessorBasePtr ProcessorGnssSingleDiff::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr)
 {
-    ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(_params, std::static_pointer_cast<SensorGnss>(sensor_ptr));
+    ProcessorGnssSingleDiffPtr prc_ptr = std::make_shared<ProcessorGnssSingleDiff>(std::static_pointer_cast<ProcessorParamsGnssSingleDiff>(_params), std::static_pointer_cast<SensorGnss>(sensor_ptr));
     prc_ptr->setName(_unique_name);
     return prc_ptr;
 }
diff --git a/src/sensor/sensor_gnss.cpp b/src/sensor/sensor_gnss.cpp
index 1c61c896f..16f2ca347 100644
--- a/src/sensor/sensor_gnss.cpp
+++ b/src/sensor/sensor_gnss.cpp
@@ -12,9 +12,11 @@ static Scalar kSecondEccentricitySquared = 6.73949674228 * 0.001;
 //static Scalar kFlattening = 1 / 298.257223563;
 
 SensorGnss::SensorGnss(StateBlockPtr _p_ptr,             //GNSS antena 3D position with respect to the car frame (base frame)
-                       StateBlockPtr _bias_ptr)          //GNSS sensor time bias
+                       StateBlockPtr _bias_ptr,          //GNSS sensor time bias
+                       IntrinsicsGnssPtr params)         //sensor params
         :
         SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0), // antena orientation has no sense in GNSS
+        params_(params),
         ENU_defined_(false),
         ENU_MAP_initialized_(false)
 {
@@ -22,20 +24,22 @@ SensorGnss::SensorGnss(StateBlockPtr _p_ptr,             //GNSS antena 3D positi
     assert((_bias_ptr == nullptr || _bias_ptr->getSize() == 1 )&& "Bad bias size");
 
     getStateBlockVec().resize(7);
-    setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, true));
-    setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, true));
-    setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, true));
-    setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, true));
+    setStateBlockPtrStatic(3, std::make_shared<StateBlock>(3, params_->translation_fixed_));
+    setStateBlockPtrStatic(4, std::make_shared<StateBlock>(1, params_->roll_fixed_));
+    setStateBlockPtrStatic(5, std::make_shared<StateBlock>(1, params_->pitch_fixed_));
+    setStateBlockPtrStatic(6, std::make_shared<StateBlock>(1, params_->yaw_fixed_));
 }
 
 SensorGnss::SensorGnss(StateBlockPtr _p_ptr,             //GNSS sensor position with respect to the car frame (base frame)
                        StateBlockPtr _bias_ptr,          //GNSS sensor time bias
+                       IntrinsicsGnssPtr params,         //sensor params
                        StateBlockPtr _t_ENU_MAP_ptr,     //ENU_MAP translation
                        StateBlockPtr _roll_ENU_MAP_ptr,  //ENU_MAP Roll
                        StateBlockPtr _pitch_ENU_MAP_ptr, //ENU_MAP Pitch
                        StateBlockPtr _yaw_ENU_MAP_ptr)   //ENU_MAP Yaw
         :
         SensorBase("GNSS", _p_ptr, nullptr, _bias_ptr, 0),// antena orientation has no sense in GNSS
+        params_(params),
         ENU_defined_(false),
         ENU_MAP_initialized_(false)
 {
@@ -105,6 +109,8 @@ void SensorGnss::setEcefEnu(const Eigen::Vector3s& _t_ECEF_ENU)
 {
     computeEnuEcef(_t_ECEF_ENU, R_ENU_ECEF_, t_ENU_ECEF_);
     ENU_defined_ = true;
+
+    WOLF_INFO("SensorGnss: ECEF-ENU initialized.")
 }
 
 void SensorGnss::setEnuEcef(const Eigen::Matrix3s& _R_ENU_ECEF, const Eigen::Vector3s& _t_ENU_ECEF)
@@ -145,7 +151,90 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
                                                    getEnuMapPitchState()->getState()(0),
                                                    getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>();
         t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
-        this->getEnuMapTranslationState()->setState(t_ENU_MAP);
+
+        // set translation state
+        setEnuMapTranslationState(t_ENU_MAP);
+
+        //std::cout << "-----------------------------------------\n";
+        //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
+        //std::cout << "t_ECEF_antena2: " << _t_ECEF_antena2.transpose() << std::endl;
+        //std::cout << "v_ENU: " << v_ENU.transpose() << std::endl;
+        //std::cout << "theta_ENU: " << theta_ENU << std::endl;
+        //std::cout << "t_MAP_antena1: " << t_MAP_antenaENU.transpose() << std::endl;
+        //std::cout << "t_MAP_antena2: " << t_MAP_antena2.transpose() << std::endl;
+        //std::cout << "v_MAP: " << v_MAP.transpose() << std::endl;
+        //std::cout << "theta_MAP: " << theta_MAP << std::endl;
+        //std::cout << "yaw set: " << yaw << std::endl;
+        //std::cout << "t_ENU1_origin: " << t_ENU_MAP.transpose() << std::endl;
+        //std::cout << "-----checks\n";
+        //std::cout << "R(-yaw) * v_ENU: " << (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>()).transpose() << std::endl;
+        //std::cout << "should be: " << v_MAP.transpose() << std::endl;
+        //std::cout << "atan2(R(-yaw) * v_ENU): " << atan2((Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(1), (Eigen::Rotation2D<Scalar>(-yaw) * v_ENU.head<2>())(0)) << std::endl;
+        //std::cout << "should be: " << atan2(v_MAP(1),v_MAP(0)) << std::endl;
+        //std::cout << "R(yaw) * v_MAP: " << (Eigen::Rotation2D<Scalar>(yaw) * v_MAP).transpose() << std::endl;
+        //std::cout << "should be: " << v_ENU.head<2>().transpose() << std::endl;
+        //std::cout << "atan2(R(yaw) * v_MAP): " << atan2((Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(1),(Eigen::Rotation2D<Scalar>(yaw) * v_MAP)(0)) << std::endl;
+        //std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl;
+        //std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl;
+        //std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl;
+
+        WOLF_INFO("SensorGnss: ENU-MAP initialized.")
+    }
+    // 3D
+    else
+    {
+        //TODO
+        std::runtime_error("not implemented yet");
+    }
+}
+
+void SensorGnss::initializeEnuMapYaw(const Eigen::VectorXs& _pose_MAP_frame1, const Eigen::VectorXs& _pose_MAP_frame2,
+                                     const Eigen::Vector3s& _v_ECEF_antena1_antena2)
+{
+    assert(ENU_defined_ && "initializing ENU-MAP yaw without ENU defined");
+
+    Eigen::Vector3s t_MAP_antena1 = _pose_MAP_frame1.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame1(2)) * getP()->getState().head<2>();
+    Eigen::Vector3s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+    Eigen::Vector3s v_MAP_antena1_antena2 = t_MAP_antena2 - t_MAP_antena1;
+    Eigen::Vector3s v_ENU_antena1_antena2 = R_ENU_ECEF_ * (_v_ECEF_antena1_antena2);
+
+    Scalar theta_ENU = atan2(v_ENU_antena1_antena2(1),v_ENU_antena1_antena2(0));
+    Scalar theta_MAP = atan2(v_MAP_antena1_antena2(1),v_MAP_antena1_antena2(0));
+    Scalar yaw_ENU_MAP = theta_ENU-theta_MAP;
+
+    this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw_ENU_MAP));
+
+    Eigen::Matrix3s R_ENU_ECEF;//, R_ENU2_ECEF;
+    Eigen::Vector3s t_ENU_ECEF;//, t_ENU2_ECEF;
+    computeEnuEcef(_t_ECEF_antenaENU, R_ENU_ECEF, t_ENU_ECEF);
+    //computeENU_ECEF(_t_ECEF_antena2, R_ENU2_ECEF, t_ENU2_ECEF);
+
+    // compute fix vector (from 1 to 2) in ENU coordinates
+    Eigen::Vector3s v_ENU = R_ENU_ECEF * (_t_ECEF_antena2 - _t_ECEF_antenaENU);
+
+    // 2D
+    if (getProblem()->getDim() == 2)
+    {
+        // compute antena vector (from 1 to 2) in MAP
+        Eigen::Vector2s t_MAP_antenaENU = _pose_MAP_frameENU.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frameENU(2)) * getP()->getState().head<2>();
+        Eigen::Vector2s t_MAP_antena2 = _pose_MAP_frame2.head<2>() + Eigen::Rotation2D<Scalar>(_pose_MAP_frame2(2)) * getP()->getState().head<2>();
+        Eigen::Vector2s v_MAP =  t_MAP_antena2 - t_MAP_antenaENU;
+
+        // initialize yaw
+        Scalar theta_ENU = atan2(v_ENU(1),v_ENU(0));
+        Scalar theta_MAP = atan2(v_MAP(1),v_MAP(0));
+        Scalar yaw = theta_ENU-theta_MAP;
+        this->getEnuMapYawState()->setState(Eigen::Vector1s(yaw));
+
+        // initialize translation
+        Eigen::Vector3s t_ENU_MAP(Eigen::Vector3s::Zero());
+        Eigen::Matrix2s R_ENU_MAP = computeREnuMap(getEnuMapRollState()->getState()(0),
+                                                   getEnuMapPitchState()->getState()(0),
+                                                   getEnuMapYawState()->getState()(0)).topLeftCorner<2,2>();
+        t_ENU_MAP.head<2>() = -R_ENU_MAP * t_MAP_antenaENU;
+
+        // set translation state
+        setEnuMapTranslationState(t_ENU_MAP);
 
         //std::cout << "-----------------------------------------\n";
         //std::cout << "t_ECEF_antenaENU: " << _t_ECEF_antenaENU.transpose() << std::endl;
@@ -169,6 +258,8 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
         //std::cout << "should be: " << atan2(v_ENU(1),v_ENU(0)) << std::endl;
         //std::cout << "v_ENU.norm(): " << v_ENU.norm() << std::endl;
         //std::cout << "v_MAP.norm(): " << v_MAP.norm() << std::endl;
+
+        WOLF_INFO("SensorGnss: ENU-MAP initialized.")
     }
     // 3D
     else
@@ -176,6 +267,11 @@ void SensorGnss::initializeEnuMap(const Eigen::VectorXs& _pose_MAP_frameENU, con
         //TODO
         std::runtime_error("not implemented yet");
     }
+}
+
+void SensorGnss::setEnuMapTranslationState(const Eigen::Vector3s& t_ENU_MAP)
+{
+    this->getEnuMapTranslationState()->setState(t_ENU_MAP);
 
     ENU_MAP_initialized_ = true;
 }
@@ -191,7 +287,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
     StateBlockPtr p_ptr = std::make_shared<StateBlock>(_extrinsics.head(3), intrinsics_gnss_ptr->extrinsics_fixed_);
 
     if (_extrinsics.size() == 3)
-        sen = std::make_shared<SensorGnss>(p_ptr, nullptr);
+        sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr);
     else
     {
         StateBlockPtr p_ptr             = std::make_shared<StateBlock>(_extrinsics.head<3>(),     intrinsics_gnss_ptr->extrinsics_fixed_);
@@ -200,7 +296,7 @@ SensorBasePtr SensorGnss::create(const std::string& _unique_name, const Eigen::V
         StateBlockPtr pitch_ptr         = std::make_shared<StateBlock>(_extrinsics.segment<1>(7), intrinsics_gnss_ptr->pitch_fixed_);
         StateBlockPtr yaw_ptr           = std::make_shared<StateBlock>(_extrinsics.segment<1>(8), intrinsics_gnss_ptr->yaw_fixed_);
 
-        sen = std::make_shared<SensorGnss>(p_ptr, nullptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr);
+        sen = std::make_shared<SensorGnss>(p_ptr, nullptr, intrinsics_gnss_ptr, translation_ptr, roll_ptr, pitch_ptr, yaw_ptr);
     }
 
     sen->setName(_unique_name);
diff --git a/test/gtest_factor_gnss_fix_2D.cpp b/test/gtest_factor_gnss_fix_2D.cpp
index bf783d30e..1f3330a89 100644
--- a/test/gtest_factor_gnss_fix_2D.cpp
+++ b/test/gtest_factor_gnss_fix_2D.cpp
@@ -100,7 +100,7 @@ TEST(FactorGnssFix2DTest, configure_tree)
     ceres_mgr_ptr->getSolverOptions().max_num_iterations = 100;
 
     // Configure sensor and processor
-    gnss_sensor_ptr->getEnuMapTranslationState()->setState(t_enu_map);
+    gnss_sensor_ptr->setEnuMapTranslationState(t_enu_map);
     gnss_sensor_ptr->getEnuMapYawState()->setState(o_enu_map);
     gnss_sensor_ptr->getEnuMapRollState()->fix();
     gnss_sensor_ptr->getEnuMapPitchState()->fix();
@@ -117,8 +117,8 @@ TEST(FactorGnssFix2DTest, configure_tree)
     problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0);
 
     // Create & process GNSS Fix capture
-    // CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
-    CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(CaptureBase::emplace<CaptureGnssFix>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity()));
+    auto cap = CaptureBase::emplace<CaptureGnssFix>(nullptr, TimeStamp(0), gnss_sensor_ptr, t_ecef_antena, 1e-3*Matrix3s::Identity());
+    CaptureGnssFixPtr cap_gnss_ptr = std::static_pointer_cast<CaptureGnssFix>(cap);
     gnss_sensor_ptr->process(cap_gnss_ptr);
 
     // Checks
-- 
GitLab