From 2f7e472edbd741ef0e8bdba2c69838301b4d796e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Thu, 11 Apr 2019 12:11:09 +0200 Subject: [PATCH] Removed getCovariance functions that do not return bool --- include/base/frame_base.h | 1 - include/base/landmark/landmark_base.h | 1 - include/base/problem.h | 4 +-- src/frame_base.cpp | 5 --- src/landmark/landmark_base.cpp | 5 --- src/problem.cpp | 51 ++++++++++++--------------- 6 files changed, 24 insertions(+), 43 deletions(-) diff --git a/include/base/frame_base.h b/include/base/frame_base.h index be97eb80a..0177bd8e9 100644 --- a/include/base/frame_base.h +++ b/include/base/frame_base.h @@ -113,7 +113,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase void getState(Eigen::VectorXs& _state) const; unsigned int getSize() const; bool getCovariance(Eigen::MatrixXs& _cov) const; - Eigen::MatrixXs getCovariance() const; // Wolf tree access --------------------------------------------------- public: diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h index 9019ee85e..e3b61d65b 100644 --- a/include/base/landmark/landmark_base.h +++ b/include/base/landmark/landmark_base.h @@ -72,7 +72,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma Eigen::VectorXs getState() const; void getState(Eigen::VectorXs& _state) const; bool getCovariance(Eigen::MatrixXs& _cov) const; - Eigen::MatrixXs getCovariance() const; protected: virtual void removeStateBlocks(); diff --git a/include/base/problem.h b/include/base/problem.h index 84b51c2a6..a9f8e5e4a 100644 --- a/include/base/problem.h +++ b/include/base/problem.h @@ -236,10 +236,8 @@ class Problem : public std::enable_shared_from_this<Problem> bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov); bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0); bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance); - Eigen::MatrixXs getFrameCovariance(FrameBaseConstPtr _frame_ptr); - Eigen::MatrixXs getLastKeyFrameCovariance(); + bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance); bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance); - Eigen::MatrixXs getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr); // Solver management ---------------------------------------- diff --git a/src/frame_base.cpp b/src/frame_base.cpp index ccd51dc7f..3a7b4eed4 100644 --- a/src/frame_base.cpp +++ b/src/frame_base.cpp @@ -221,11 +221,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const return getProblem()->getFrameCovariance(shared_from_this(), _cov); } -Eigen::MatrixXs FrameBase::getCovariance() const -{ - return getProblem()->getFrameCovariance(shared_from_this()); -} - FrameBasePtr FrameBase::getPreviousFrame() const { //std::cout << "finding previous frame of " << this->frame_id_ << std::endl; diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index d1fb63043..c9f2e125b 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -94,11 +94,6 @@ void LandmarkBase::registerNewStateBlocks() } } -Eigen::MatrixXs LandmarkBase::getCovariance() const -{ - return getProblem()->getLandmarkCovariance(shared_from_this()); -} - bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const { return getProblem()->getLandmarkCovariance(shared_from_this(), _cov); diff --git a/src/problem.cpp b/src/problem.cpp index b9cd0a1e6..0b3728d2c 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -564,6 +564,16 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& const auto& state_bloc_vec = _frame_ptr->getStateBlockVec(); + // computing size + SizeEigen sz = 0; + for (const auto& sb : state_bloc_vec) + if (sb) + sz += sb->getSize(); + + // resizing + _covariance = Eigen::MatrixXs(sz, sz); + + // filling covariance for (const auto& sb_i : state_bloc_vec) { if (sb_i) @@ -583,23 +593,10 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& return success; } -Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr) -{ - SizeEigen sz = 0; - for (const auto& sb : _frame_ptr->getStateBlockVec()) - if (sb) - sz += sb->getSize(); - - Eigen::MatrixXs covariance(sz, sz); - - getFrameCovariance(_frame_ptr, covariance); - return covariance; -} - -Eigen::MatrixXs Problem::getLastKeyFrameCovariance() +bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) { FrameBasePtr frm = getLastKeyFrame(); - return getFrameCovariance(frm); + return getFrameCovariance(frm, cov); } bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) @@ -609,6 +606,17 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec(); + // computing size + SizeEigen sz = 0; + for (const auto& sb : state_bloc_vec) + if (sb) + sz += sb->getSize(); + + // resizing + _covariance = Eigen::MatrixXs(sz, sz); + + // filling covariance + for (const auto& sb_i : state_bloc_vec) { if (sb_i) @@ -628,19 +636,6 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M return success; } -Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr) -{ - SizeEigen sz = 0; - for (const auto& sb : _landmark_ptr->getStateBlockVec()) - if (sb) - sz += sb->getSize(); - - Eigen::MatrixXs covariance(sz, sz); - - getLandmarkCovariance(_landmark_ptr, covariance); - return covariance; -} - MapBasePtr Problem::getMap() { return map_ptr_; -- GitLab