diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 023990a0b5df42cc28c9989b3627a97c14a36d36..6130543ed715cf48dae1e13d0913adb92d073b35 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 1a098cfd9eb2ecf70f69715a7e3629514238d4eb..9d52baaa2e57fa01c369373dc90051a4e0098029 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 d2b75c8a2c0803bceeb97ff4d6f6d0ce120ae5e7..7fec21eaf3ad252817d224b9a4e2a111b38ccecd 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 d0e71cea5e9548dfefc38ae68f5d9522f4fa0f4a..a1df458e49f2f3f60b5e6b5c6b38a2986cea4112 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 386779f1e0165cf6f744ecdac9f34bbf90d073ea..9e56aaf12257b42e480a0da90f70e7e38240e7c9 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();