diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index aafa35a21b634b89bcecbf057de5674967bcafcc..f128ae15169450a1091b2b065d97f27053b71d97 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -129,6 +129,7 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
unsigned int getSize() const;
+ unsigned int getLocalSize() const;
bool getCovariance(Eigen::MatrixXs& _cov) const;
// Wolf tree access ---------------------------------------------------
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index 5e11a2d4f5bda019bd7cf96698371d5812d61f00..d0e71cea5e9548dfefc38ae68f5d9522f4fa0f4a 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -90,9 +90,9 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
//frame state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKeyOrAux())
- for (const auto& pair_key_sb : fr_ptr->getStateBlockMap())
+ for (const auto& key : wolf_problem_->getFrameStructure())
{
- const auto& sb = pair_key_sb.second;
+ const auto& sb = fr_ptr->getStateBlock(key);
all_state_blocks.push_back(sb);
}
@@ -117,14 +117,14 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// first create a vector containing all state blocks
for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKeyOrAux())
- for (auto pair_key_sb : fr_ptr->getStateBlockMap())
- for(auto pair_key_sb2 : fr_ptr->getStateBlockMap())
+ for (const auto& key1 : wolf_problem_->getFrameStructure())
+ for (const auto& key2 : wolf_problem_->getFrameStructure())
{
- const auto& sb = pair_key_sb.second;
- const auto& sb2 = pair_key_sb2.second;
- state_block_pairs.emplace_back(sb, sb2);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
- if (sb == sb2)
+ const auto& sb1 = fr_ptr->getStateBlock(key1);
+ const auto& sb2 = fr_ptr->getStateBlock(key2);
+ state_block_pairs.emplace_back(sb1, sb2);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
+ if (sb1 == sb2)
break;
}
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 6e05103636414770603493105a1e632d280206cd..446ced4af7c50af5d13e3ab68053c47f9a556f82 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -152,8 +152,6 @@ void FrameBase::remove(bool viral_remove_empty_parent)
// Remove Frame State Blocks
if ( isKeyOrAux() )
removeStateBlocks();
-
-// std::cout << "Removed F" << id() << std::endl;
}
}
@@ -176,33 +174,18 @@ void FrameBase::registerNewStateBlocks()
void FrameBase::removeStateBlocks()
{
- for (auto pair_key_sb : getStateBlockMap())
+ for (const char& key : structure_) // note: key is a char
{
- auto& key = pair_key_sb.first;
- auto& sbp = pair_key_sb.second;
+ auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
- removeStateBlock(key);
}
+ removeStateBlock(key);
}
-
-
-// for (unsigned int i = 0; i < state_block_map_.size(); i++)
-// {
-// StateBlockPtr sbp = getStateBlock(i);
-// if (sbp != nullptr)
-// {
-// if (getProblem() != nullptr)
-// {
-// getProblem()->notifyStateBlock(sbp,REMOVE);
-// }
-// setStateBlock(i, nullptr);
-// }
-// }
}
void FrameBase::setNonEstimated()
@@ -276,26 +259,35 @@ void FrameBase::setAux()
void FrameBase::setState(const Eigen::VectorXs& _state)
{
int size = getSize();
-// for (const auto & pair_key_sb : getStateBlockMap())
-// size += pair_key_sb.second->getSize();
-// for(unsigned int i = 0; i<state_block_vec_.size(); i++){
-// size += (state_block_vec_[i]==nullptr ? 0 : state_block_vec_[i]->getSize());
-// }
//State Vector size must be lower or equal to frame state size :
- // example : PQVBB -> if we initialize only position and orientation due to use of processorOdom3D
- assert(_state.size() <= size && "In FrameBase::setState wrong state size");
+// example : POV -> if we initialize only position and orientation due to use of processorOdom3D
+ assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
- const unsigned int _st_size = _state.size();
+ for (const char& key : structure_) // note: key is a char
+ {
+ const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
+
+ sb->setState(_state.segment(index, sb->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
+ index += sb->getSize();
+ }
+
+}
+
+void FrameBase::getState(Eigen::VectorXs& _state) const
+{
+ _state.resize(getSize());
+
+ unsigned int index = 0;
+ for (const char& key : structure_) // note: key is a char
+ {
+ const auto& sb = getStateBlock(key); // note: we need a string, not a char. Use string constructor s(1,char).
+
+ _state.segment(index,sb->getSize()) = sb->getState();
+ index += sb->getSize();
+ }
- //initialize the FrameBase StateBlocks while StateBlocks list's end not reached and input state_size can go further
- for (auto& pair_key_sb : getStateBlockMap())
- if (index < _st_size)
- {
- pair_key_sb.second->setState(_state.segment(index, pair_key_sb.second->getSize()), isKeyOrAux()); // do not notify if state block is not estimated by the solver
- index += pair_key_sb.second->getSize();
- }
}
Eigen::VectorXs FrameBase::getState() const
@@ -307,23 +299,19 @@ Eigen::VectorXs FrameBase::getState() const
return state;
}
-void FrameBase::getState(Eigen::VectorXs& _state) const
+unsigned int FrameBase::getSize() const
{
- _state.resize(getSize());
-
- SizeEigen index = 0;
+ unsigned int size = 0;
for (const auto& pair_key_sb : getStateBlockMap())
- {
- _state.segment(index,pair_key_sb.second->getSize()) = pair_key_sb.second->getState();
- index += pair_key_sb.second->getSize();
- }
+ size += pair_key_sb.second->getSize();
+ return size;
}
-unsigned int FrameBase::getSize() const
+unsigned int FrameBase::getLocalSize() const
{
unsigned int size = 0;
for (const auto& pair_key_sb : getStateBlockMap())
- size += pair_key_sb.second->getSize();
+ size += pair_key_sb.second->getLocalSize();
return size;
}
@@ -334,10 +322,6 @@ bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
FrameBasePtr FrameBase::getPreviousFrame() const
{
- //std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
- if (getTrajectory() == nullptr)
- //std::cout << "This Frame is not linked to any trajectory" << std::endl;
-
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
@@ -348,17 +332,14 @@ FrameBasePtr FrameBase::getPreviousFrame() const
f_it++;
if (f_it != getTrajectory()->getFrameList().rend())
{
- //std::cout << "previous frame found!" << std::endl;
return *f_it;
}
else
{
- //std::cout << "previous frame not found!" << std::endl;
return nullptr;
}
}
}
- //std::cout << "previous frame not found!" << std::endl;
return nullptr;
}
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 687a1565cd331827268c2cbd5eeb683944363a3b..9f671ca79b73d6a9d74eaef39d6b7000749064ca 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -628,32 +628,26 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, co
bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const
{
bool success(true);
- int i = 0, j = 0;
-
- const auto& state_bloc_map = _frame_ptr->getStateBlockMap();
-
- // computing size
- SizeEigen sz = 0;
- for (const auto& pair_key_sb : state_bloc_map)
- sz += pair_key_sb.second->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ SizeEigen sz = _frame_ptr->getLocalSize();
+ _covariance.resize(sz, sz);
// filling covariance
- for (const auto& pair_key_sb_i : state_bloc_map)
+ int i = 0, j = 0;
+ for (const auto& key_i : getFrameStructure())
{
- const auto& sb_i = pair_key_sb_i.second;
+ const auto& sb_i = _frame_ptr->getStateBlock(key_i);
j = 0;
- for (const auto& pair_key_sb_j : state_bloc_map)
+ for (const auto& key_j : getFrameStructure())
{
- const auto& sb_j = pair_key_sb_j.second;
+ const auto& sb_j = _frame_ptr->getStateBlock(key_j);
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
i += sb_i->getLocalSize();
-
}
+
return success;
}