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