diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp
index 5500b724a95cf12984d12b1327749833a5ff8acc..793d409e4c7522ee2ebcc18ad01ec61cbbf1bbbc 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 db934ca13174443de5053e3a427206de41799c18..d41a1a273a7aa50313ebcab126b0f9f38997406c 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 c04043d857a43f6db5391b2c463b5328ab331a90..9d6a62c6715436fcb3182443da1ad9af91b4c08a 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 f4c53fa10da0908a5cd82ef3c83fe9796070abee..8ff4c63f114594d5b4c5de2a2900e7741c172e42 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 c8a04dcd16131c9e0549d5b34ed9d81d4c353ec8..4c8df1549d4c59a02b3d9bb754eca4e2b6b0533b 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 4f280a6ff5482d8d02961438b0aaa119990bf78d..8283b62b8ef35e591dad8533256a517ad65f7718 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 225a49326c54d4e743bd7cd6c43bf839785b2e3d..1192372ff97072462258a560a7b64bdfc0776b14 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 c0004ad3992e256e7a3e5d883ebaea1707295775..e883a1597c26f8418c06348a4b5b3db50cad01f4 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 8c21ca410b7b9111f11994ebad9f74cfc91cfdba..267a202f370c59e92371136fb6c6cf587f9985f0 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;