Use structure in for loops. Add localSize()

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 ---------------------------------------------------

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;
                         }
 

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;
 }
 

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;
 }