compiling

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())
     {

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())
     {

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
          **/

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

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

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

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

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;

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;