From 92ecd6cdb1f0ec6449ce467df9928d626f4be18a Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Wed, 6 Apr 2022 12:33:51 +0200
Subject: [PATCH] Revert "Merge branch '21-adapt-to-const-nonconst-new-api'
 into 'devel'"

This reverts merge request !30
---
 include/gnss/sensor/sensor_gnss.h        | 40 +++++-------------------
 src/processor/processor_gnss_tdcp.cpp    |  8 ++---
 src/processor/processor_tracker_gnss.cpp |  8 ++---
 3 files changed, 16 insertions(+), 40 deletions(-)

diff --git a/include/gnss/sensor/sensor_gnss.h b/include/gnss/sensor/sensor_gnss.h
index fece26836..03fc7a2d4 100644
--- a/include/gnss/sensor/sensor_gnss.h
+++ b/include/gnss/sensor/sensor_gnss.h
@@ -110,14 +110,10 @@ class SensorGnss : public SensorBase
         ~SensorGnss() override;
 
         // Gets
-        StateBlockConstPtr getEnuMapTranslation() const;
-        StateBlockPtr getEnuMapTranslation();
-        StateBlockConstPtr getEnuMapRoll() const;
-        StateBlockPtr getEnuMapRoll();
-        StateBlockConstPtr getEnuMapPitch() const;
-        StateBlockPtr getEnuMapPitch();
-        StateBlockConstPtr getEnuMapYaw() const;
-        StateBlockPtr getEnuMapYaw();
+        StateBlockPtr getEnuMapTranslation() const;
+        StateBlockPtr getEnuMapRoll() const;
+        StateBlockPtr getEnuMapPitch() const;
+        StateBlockPtr getEnuMapYaw() const;
         const Eigen::Matrix3d& getREnuEcef() const;
         const Eigen::Vector3d& gettEnuEcef() const;
         Eigen::Matrix3d getREnuMap() const;
@@ -182,42 +178,22 @@ inline bool SensorGnss::isEnuModeAuto() const
     return params_->ENU_mode == "auto";
 }
 
-inline StateBlockConstPtr SensorGnss::getEnuMapTranslation() const
+inline StateBlockPtr SensorGnss::getEnuMapTranslation() const
 {
     return getStateBlock('t');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapTranslation()
-{
-    return getStateBlock('t');
-}
-
-inline StateBlockConstPtr SensorGnss::getEnuMapRoll() const
-{
-    return getStateBlock('r');
-}
-
-inline StateBlockPtr SensorGnss::getEnuMapRoll()
+inline StateBlockPtr SensorGnss::getEnuMapRoll() const
 {
     return getStateBlock('r');
 }
 
-inline StateBlockConstPtr SensorGnss::getEnuMapPitch() const
+inline StateBlockPtr SensorGnss::getEnuMapPitch() const
 {
     return getStateBlock('p');
 }
 
-inline StateBlockPtr SensorGnss::getEnuMapPitch()
-{
-    return getStateBlock('p');
-}
-
-inline StateBlockConstPtr SensorGnss::getEnuMapYaw() const
-{
-    return getStateBlock('y');
-}
-
-inline StateBlockPtr SensorGnss::getEnuMapYaw()
+inline StateBlockPtr SensorGnss::getEnuMapYaw() const
 {
     return getStateBlock('y');
 }
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 2cd171897..c087f7074 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/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_rit = getProblem()->getTrajectory()->getFrameMap().rbegin();
-         KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
-         KF_rit++)
+    for (auto KF_it = getProblem()->getTrajectory()->rbegin();
+         KF_it != getProblem()->getTrajectory()->rend();
+         KF_it++)
     {
-        auto KF_ref = KF_rit->second;
+        auto KF_ref = *KF_it;
         if (_keyframe->getTimeStamp() < KF_ref->getTimeStamp())
             continue;
 
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 1a7aa706f..ace52948c 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/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()->getFrameMap().rbegin();
-                 KF_rit != getProblem()->getTrajectory()->getFrameMap().rend();
+            for (auto KF_rit = getProblem()->getTrajectory()->rbegin();
+                 KF_rit != getProblem()->getTrajectory()->rend();
                  KF_rit++)
             {
-                FrameBasePtr KF = KF_rit->second;
+                FrameBasePtr KF = (*KF_rit);
 
                 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->getCaptureOf(getSensor(),"CaptureGnss"));
+                auto ref_cap_gnss = std::static_pointer_cast<CaptureGnss>((*KF_rit)->getCaptureOf(getSensor(),"CaptureGnss"));
                 auto last_cap_gnss = std::static_pointer_cast<CaptureGnss>(last_ptr_);
 
                 // dt
-- 
GitLab