compiling and tests ok

include/gnss/sensor/sensor_gnss.h

@@ -110,10 +110,14 @@ class SensorGnss : public SensorBase
         ~SensorGnss() override;
 
         // Gets
-        StateBlockPtr getEnuMapTranslation() const;
-        StateBlockPtr getEnuMapRoll() const;
-        StateBlockPtr getEnuMapPitch() const;
-        StateBlockPtr getEnuMapYaw() const;
+        StateBlockConstPtr getEnuMapTranslation() const;
+        StateBlockPtr getEnuMapTranslation();
+        StateBlockConstPtr getEnuMapRoll() const;
+        StateBlockPtr getEnuMapRoll();
+        StateBlockConstPtr getEnuMapPitch() const;
+        StateBlockPtr getEnuMapPitch();
+        StateBlockConstPtr getEnuMapYaw() const;
+        StateBlockPtr getEnuMapYaw();
         const Eigen::Matrix3d& getREnuEcef() const;
         const Eigen::Vector3d& gettEnuEcef() const;
         Eigen::Matrix3d getREnuMap() const;
@@ -178,22 +182,42 @@ inline bool SensorGnss::isEnuModeAuto() const
     return params_->ENU_mode == "auto";
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
 {
     return getStateBlock('t');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapRoll() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation()
+{
+    return getStateBlock('t');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
+{
+    return getStateBlock('r');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapRoll()
 {
     return getStateBlock('r');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
 {
     return getStateBlock('p');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapYaw() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch()
+{
+    return getStateBlock('p');
+}
+
+inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
+{
+    return getStateBlock('y');
+}
+
+inline StateBlockPtr SensorGnss::getEnuMapYaw()
 {
     return getStateBlock('y');
 }

src/processor/processor_gnss_tdcp.cpp

@@ -144,11 +144,11 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe)
     }
 
     // Iterate over all KF of the trajectory with GNSS captures
-    for (auto KF_it = getProblem()->getTrajectory()->rbegin();
-         KF_it != getProblem()->getTrajectory()->rend();
-         KF_it++)
+    for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin();
+         KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
+         KF_rit++)
     {
-        auto KF_ref = *KF_it;
+        auto KF_ref = KF_rit->second;
         if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
             continue;
 

src/processor/processor_tracker_gnss.cpp

@@ -410,11 +410,11 @@ void ProcessorTrackerGnss::establishFactors()
             WOLF_DEBUG("TDCP BATCH frame ", last_ptr_->getFrame()->id());
             FactorBasePtr last_fac_ptr = nullptr;
 
-            for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
-                 KF_rit != getProblem()->getTrajectory()->rend();
+            for (auto KF_rit = getProblem()->getTrajectory()->getFrameMap().rbegin();
+                 KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
                  KF_rit++)
             {
-                FrameBasePtr KF = (*KF_rit);
+                FrameBasePtr KF = KF_rit->second;
 
                 WOLF_DEBUG("TDCP BATCH ref frame ", KF->id());
 
@@ -424,7 +424,7 @@ void ProcessorTrackerGnss::establishFactors()
                     continue;
 
                 // static cast
-                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
+                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>(KF->getCaptureOf(getSensor(),"CaptureGnss"));
                 auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
 
                 // dt