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