diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 8abc538d875763d1a8b50e82ef213420faaefc37..14f8588b7d01715a6ffb3dcd2e8be8d119aff78d 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -197,7 +197,6 @@ class Problem : public std::enable_shared_from_this<Problem> void removeMotionProvider(MotionProviderPtr proc); public: -// MotionProviderPtr getMotionProvider(); std::map<int,MotionProviderPtr>& getMotionProviderMap(); const std::map<int,MotionProviderPtr>& getMotionProviderMap() const; @@ -443,13 +442,6 @@ inline bool Problem::isPriorSet() const return prior_options_ == nullptr; } -//inline MotionProviderPtr Problem::getMotionProvider() -//{ -// if (not motion_provider_map_.empty()) -// return motion_provider_map_.begin()->second; -// return nullptr; -//} - inline std::map<int,MotionProviderPtr>& Problem::getMotionProviderMap() { return motion_provider_map_; diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 787619db07741aa4ab7fd8228aaa37787885bbd6..8cdffb0070a9f8ed8ad2f6d18c4d309ba949c833 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -969,42 +969,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M } return success; - - - -// bool success(true); -// int i = 0, j = 0; -// -// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec(); -// -// // computing size -// SizeEigen sz = 0; -// for (const auto& sb : state_bloc_vec) -// if (sb) -// sz += sb->getLocalSize(); -// -// // resizing -// _covariance = Eigen::MatrixXs(sz, sz); -// -// // filling covariance -// -// for (const auto& sb_i : state_bloc_vec) -// { -// if (sb_i) -// { -// j = 0; -// for (const auto& sb_j : state_bloc_vec) -// { -// if (sb_j) -// { -// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); -// j += sb_j->getLocalSize(); -// } -// } -// i += sb_i->getLocalSize(); -// } -// } -// return success; } MapBasePtr Problem::getMap() const