From 24ff91a18ad2451bb6375e1f15394743d94bab8b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 2 Sep 2019 01:16:07 +0200
Subject: [PATCH] Remove commented code
---
hello_wolf/hello_wolf.cpp | 3 --
hello_wolf/hello_wolf_autoconf.cpp | 3 --
include/core/landmark/landmark_base.h | 59 -----------------------
src/ceres_wrapper/ceres_manager.cpp | 2 +-
src/landmark/landmark_base.cpp | 67 ++++++---------------------
5 files changed, 14 insertions(+), 120 deletions(-)
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 023990a0b..6130543ed 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -233,9 +233,6 @@ int main()
for (auto& pair_key_sb : lmk->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
-// for (auto sb : lmk->getStateBlockVec())
-// if (sb && !sb->isFixed())
-// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index 1a098cfd9..9d52baaa2 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -226,9 +226,6 @@ int main()
for (auto& pair_key_sb : lmk->getStateBlockMap())
if (!pair_key_sb.second->isFixed())
pair_key_sb.second->setState(pair_key_sb.second->getState() + VectorXs::Random(pair_key_sb.second->getSize()) * 0.5); // We perturb A LOT !
-// for (auto sb : lmk->getStateBlockVec())
-// if (sb && !sb->isFixed())
-// sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index d2b75c8a2..7fec21eaf 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -58,23 +58,8 @@ class LandmarkBase : public NodeBase, public HasStateBlocks, public std::enable_
unsigned int id() const;
void setId(unsigned int _id);
-// // Fix / unfix
-// void fix();
-// void unfix();
-// bool isFixed() const;
-
// State blocks
-// const std::vector<StateBlockPtr>& getStateBlockVec() const;
-// std::vector<StateBlockPtr>& getStateBlockVec();
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
-// StateBlockPtr getStateBlock(unsigned int _i) const;
-// void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
-// StateBlockPtr getP() const;
-// StateBlockPtr getO() const;
-// void setP(const StateBlockPtr _p_ptr);
-// void setO(const StateBlockPtr _o_ptr);
-// Eigen::VectorXs getState() const;
-// void getState(Eigen::VectorXs& _state) const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
protected:
@@ -157,50 +142,6 @@ inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
return constrained_by_list_;
}
-//inline const std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec() const
-//{
-// return state_block_vec_;
-//}
-//
-//inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
-//{
-// return state_block_vec_;
-//}
-//
-//inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
-//{
-// // assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
-// if (_i < state_block_vec_.size())
-// return state_block_vec_[_i];
-// else
-// return nullptr;
-//}
-//
-//inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
-//{
-// state_block_vec_[_i] = _sb_ptr;
-//}
-//
-//inline StateBlockPtr LandmarkBase::getP() const
-//{
-// return getStateBlock(0);
-//}
-//
-//inline StateBlockPtr LandmarkBase::getO() const
-//{
-// return getStateBlock(1);
-//}
-//
-//inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
-//{
-// setStateBlock(0, _st_ptr);
-//}
-//
-//inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
-//{
-// setStateBlock(1, _st_ptr);
-//}
-
inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
{
descriptor_ = _descriptor;
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index d0e71cea5..a1df458e4 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -90,7 +90,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
//frame state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKeyOrAux())
- for (const auto& key : wolf_problem_->getFrameStructure())
+ for (const auto& key : fr_ptr->getStructure())
{
const auto& sb = fr_ptr->getStateBlock(key);
all_state_blocks.push_back(sb);
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 386779f1e..9e56aaf12 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -16,7 +16,7 @@ LandmarkBase::LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, State
NodeBase("LANDMARK", _type),
HasStateBlocks(""),
map_ptr_(),
- state_block_vec_(2), // allow for 2 state blocks by default. Resize in derived constructors if needed.
+ state_block_vec_(0), // Resize in derived constructors if needed.
landmark_id_(++landmark_id_count_)
{
if (_p_ptr)
@@ -60,37 +60,23 @@ void LandmarkBase::remove(bool viral_remove_empty_parent)
}
}
-//void LandmarkBase::fix()
-//{
-// for (auto sbp : state_block_vec_)
-// if (sbp != nullptr)
-// sbp->fix();
-//}
-//
-//void LandmarkBase::unfix()
-//{
-// for (auto sbp : state_block_vec_)
-// if (sbp != nullptr)
-// sbp->unfix();
-//}
-//
-//bool LandmarkBase::isFixed() const
-//{
-// bool fixed = true;
-// for (auto sb : getStateBlockVec())
-// {
-// if (sb)
-// fixed &= sb->isFixed();
-// }
-// return fixed;
-//}
-
std::vector<StateBlockPtr> LandmarkBase::getUsedStateBlockVec() const
{
std::vector<StateBlockPtr> used_state_block_vec(0);
+
+ // normal state blocks in {P,O,V,W}
+ for (const auto& key : getStructure())
+ {
+ const auto& sbp = getStateBlock(key);
+ if (sbp)
+ used_state_block_vec.push_back(sbp);
+ }
+
+ // other state blocks in a vector
for (auto sbp : state_block_vec_)
if (sbp)
used_state_block_vec.push_back(sbp);
+
return used_state_block_vec;
}
@@ -125,33 +111,6 @@ void LandmarkBase::removeStateBlocks()
}
}
-//Eigen::VectorXs LandmarkBase::getState() const
-//{
-// Eigen::VectorXs state;
-//
-// getState(state);
-//
-// return state;
-//}
-//
-//void LandmarkBase::getState(Eigen::VectorXs& _state) const
-//{
-// SizeEigen size = 0;
-// for (StateBlockPtr sb : state_block_vec_)
-// if (sb)
-// size += sb->getSize();
-//
-// _state = Eigen::VectorXs(size);
-//
-// SizeEigen index = 0;
-// for (StateBlockPtr sb : state_block_vec_)
-// if (sb)
-// {
-// _state.segment(index,sb->getSize()) = sb->getState();
-// index += sb->getSize();
-// }
-//}
-
YAML::Node LandmarkBase::saveToYaml() const
{
YAML::Node node;
@@ -162,7 +121,7 @@ YAML::Node LandmarkBase::saveToYaml() const
node["position"] = getP()->getState();
node["position fixed"] = getP()->isFixed();
}
- if (state_block_vec_.size() > 1 && getO() != nullptr)
+ if (getO() != nullptr)
{
node["orientation"] = getO()->getState();
node["orientation fixed"] = getO()->isFixed();
--
GitLab