From 1193c14fb2bc190d32d975de3091beb66b6cc155 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Tue, 5 Apr 2022 18:14:12 +0200
Subject: [PATCH] compiling

---
 demos/hello_wolf/hello_wolf.cpp           |  6 +++---
 demos/hello_wolf/hello_wolf_autoconf.cpp  |  6 +++---
 test/dummy/factor_feature_dummy.h         |  3 ++-
 test/dummy/factor_landmark_dummy.h        | 14 +++++++++-----
 test/gtest_factor_base.cpp                |  5 +++--
 test/gtest_factor_diff_drive.cpp          |  2 +-
 test/gtest_odom_2d.cpp                    |  6 ++++--
 test/gtest_solver_ceres_multithread.cpp   |  2 +-
 test/gtest_solver_manager_multithread.cpp |  2 +-
 9 files changed, 27 insertions(+), 19 deletions(-)

diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 5500b724a..793d409e4 100644
--- a/demos/hello_wolf/hello_wolf.cpp
+++ b/demos/hello_wolf/hello_wolf.cpp
@@ -253,11 +253,11 @@ int main()
     // GET COVARIANCES of all states
     WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto& kf : *problem->getTrajectory())
+    for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
     {
         Eigen::MatrixXd cov;
-        kf->getCovariance(cov);
-        WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
+        kf_pair.second->getCovariance(cov);
+        WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov);
     }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp
index db934ca13..d41a1a273 100644
--- a/demos/hello_wolf/hello_wolf_autoconf.cpp
+++ b/demos/hello_wolf/hello_wolf_autoconf.cpp
@@ -243,11 +243,11 @@ int main()
     // GET COVARIANCES of all states
     WOLF_INFO("======== COVARIANCES OF SOLVED PROBLEM =======")
     ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
-    for (auto& kf : *problem->getTrajectory())
+    for (auto& kf_pair : problem->getTrajectory()->getFrameMap())
     {
         Eigen::MatrixXd cov;
-        kf->getCovariance(cov);
-        WOLF_INFO("KF", kf->id(), "_cov = \n", cov);
+        kf_pair.second->getCovariance(cov);
+        WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov);
     }
     for (auto& lmk : problem->getMap()->getLandmarkList())
     {
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index c04043d85..9d6a62c67 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -53,7 +53,8 @@ class FactorFeatureDummy : public FactorBase
 
         /** \brief Returns a vector of pointers to the states in which this factor depends
          **/
-        std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
+        std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockConstPtr>(0);}
+        std::vector<StateBlockPtr> getStateBlockPtrVector() override {return std::vector<StateBlockPtr>(0);}
 
         /** \brief Returns the factor residual size
          **/
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index f4c53fa10..8ff4c63f1 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -42,14 +42,14 @@ class FactorLandmarkDummy : public FactorBase
         /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
         **/
         bool evaluate(double const* const* parameters,
-                              double* residuals,
-                              double** jacobians) const override {return true;};
+                      double* residuals,
+                      double** jacobians) const override {return true;};
 
         /** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
          **/
         void evaluate(const std::vector<const double*>& _states_ptr,
-                              Eigen::VectorXd& residual_,
-                              std::vector<Eigen::MatrixXd>& jacobians_) const override {};
+                      Eigen::VectorXd& residual_,
+                      std::vector<Eigen::MatrixXd>& jacobians_) const override {};
 
         /** \brief Returns the jacobians computation method
          **/
@@ -57,7 +57,11 @@ class FactorLandmarkDummy : public FactorBase
 
         /** \brief Returns a vector of pointers to the states in which this factor depends
          **/
-        std::vector<StateBlockPtr> getStateBlockPtrVector() const override
+        std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override
+        {
+            return std::vector<StateBlockConstPtr>(0);
+        }
+        std::vector<StateBlockPtr> getStateBlockPtrVector() override
         {
             return std::vector<StateBlockPtr>(0);
         }
diff --git a/test/gtest_factor_base.cpp b/test/gtest_factor_base.cpp
index c8a04dcd1..4c8df1549 100644
--- a/test/gtest_factor_base.cpp
+++ b/test/gtest_factor_base.cpp
@@ -101,8 +101,9 @@ class FactorDummy : public FactorBase
                               Eigen::VectorXd& _residual,
                               std::vector<Eigen::MatrixXd>& _jacobians) const override {}
         JacobianMethod getJacobianMethod() const override {return JacobianMethod::JAC_ANALYTIC;}
-        std::vector<StateBlockPtr> getStateBlockPtrVector() const override {std::vector<StateBlockPtr> v; return v;}
-        std::vector<unsigned int> getStateSizes() const override {std::vector<unsigned int> v; return v;}
+        std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockConstPtr>();}
+        std::vector<StateBlockPtr> getStateBlockPtrVector() override {return std::vector<StateBlockPtr>();}
+        std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>();}
         unsigned int getSize() const override {return 0;}
 
 };
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 4f280a6ff..8283b62b8 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -83,7 +83,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
             Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
         }
 
-        VectorXd getCalibration (const CaptureBasePtr _capture) const override
+        VectorXd getCalibration (const CaptureBaseConstPtr _capture) const override
         {
             return Base::getCalibration(_capture);
         }
diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp
index 225a49326..1192372ff 100644
--- a/test/gtest_odom_2d.cpp
+++ b/test/gtest_odom_2d.cpp
@@ -95,8 +95,9 @@ void show(const ProblemPtr& problem)
     using std::endl;
     cout << std::setprecision(4);
 
-    for (FrameBasePtr F : *problem->getTrajectory())
+    for (auto F_pair : problem->getTrajectory()->getFrameMap())
     {
+        auto F = F_pair.second;
         cout << "----- Key Frame " << F->id() << " -----" << endl;
         if (!F->getCaptureList().empty())
         {
@@ -324,8 +325,9 @@ TEST(Odom2d, VoteForKfAndSolve)
     // Check the 3 KFs' states and covariances
     MatrixXd computed_cov;
     int n = 0;
-    for (auto F : *problem->getTrajectory())
+    for (auto F_pair : problem->getTrajectory()->getFrameMap())
     {
+        auto F = F_pair.second;
         ASSERT_POSE2d_APPROX(F->getStateVector("PO"), integrated_pose_vector[n]   , 1e-6);
         ASSERT_TRUE         (F->getCovariance(computed_cov));
         ASSERT_MATRIX_APPROX(computed_cov , integrated_cov_vector[n]    , 1e-6);
diff --git a/test/gtest_solver_ceres_multithread.cpp b/test/gtest_solver_ceres_multithread.cpp
index c0004ad39..e883a1597 100644
--- a/test/gtest_solver_ceres_multithread.cpp
+++ b/test/gtest_solver_ceres_multithread.cpp
@@ -85,7 +85,7 @@ TEST(SolverCeresMultithread, MultiThreadingTruncatedNotifications)
         ts += 1.0;
 
         if (P->getTrajectory()->getFrameMap().size() > 10)
-            (*P->getTrajectory()->begin())->remove();
+            P->getTrajectory()->getFirstFrame()->remove();
 
         if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
             break;
diff --git a/test/gtest_solver_manager_multithread.cpp b/test/gtest_solver_manager_multithread.cpp
index 8c21ca410..267a202f3 100644
--- a/test/gtest_solver_manager_multithread.cpp
+++ b/test/gtest_solver_manager_multithread.cpp
@@ -80,7 +80,7 @@ TEST(SolverManagerMultithread, MultiThreadingTruncatedNotifications)
         ts += 1.0;
 
         if (P->getTrajectory()->getFrameMap().size() > 10)
-            (*P->getTrajectory()->begin())->remove();
+            P->getTrajectory()->getFirstFrame()->remove();
 
         if (std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::high_resolution_clock::now() - start).count() > Dt)
             break;
-- 
GitLab