Use getStateBlockVec() whenever possible

src/problem/problem.cpp

@@ -677,13 +677,11 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
 
     // filling covariance
     int i = 0, j = 0;
-    for (const auto& key_i : _frame_ptr->getStructure())
+    for (const auto& sb_i : _frame_ptr->getStateBlockVec())
     {
-        const auto& sb_i = _frame_ptr->getStateBlock(key_i);
         j = 0;
-        for (const auto& key_j : _frame_ptr->getStructure())
+        for (const auto& sb_j : _frame_ptr->getStateBlockVec())
         {
-            const auto& sb_j = _frame_ptr->getStateBlock(key_j);
             success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
             j += sb_j->getLocalSize();
         }
@@ -715,13 +713,11 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
 
     // filling covariance
     int i = 0, j = 0;
-    for (const auto& key_i : _landmark_ptr->getStructure())
+    for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
     {
-        const auto& sb_i = _landmark_ptr->getStateBlock(key_i);
         j = 0;
-        for (const auto& key_j : _landmark_ptr->getStructure())
+        for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
         {
-            const auto& sb_j = _landmark_ptr->getStateBlock(key_j);
             success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
             j += sb_j->getLocalSize();
         }
@@ -907,9 +903,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             else if (state_blocks)
             {
                 cout << "    sb:" << (S->isExtrinsicDynamic() ? "[Dyn," : "[Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
-                for (auto& key : S->getStructure())
+                for (auto& sb : S->getStateBlockVec())
                 {
-                    auto sb = S->getStateBlockPtrStatic(key);
                     if (sb != nullptr)
                         cout << " " << (sb->isFixed() ? "Fix" : "Est");
                 }
@@ -977,9 +972,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             if (state_blocks)
             {
                 cout << "    sb:";
-                for (const auto& key : F->getStructure())
+                for (const auto& sb : F->getStateBlockVec())
                 {
-                    const auto& sb = F->getStateBlock(key);
                     cout << " " << (sb->isFixed() ? "Fix" : "Est");
                 }
                 cout << endl;
@@ -1020,9 +1014,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
                     cout << endl;
 
                     if (state_blocks)
-                        for (const auto& key : C->HasStateBlocks::getStructure())
+                        for (const auto& sb : C->getStateBlockVec())
                         {
-                            auto sb = C->HasStateBlocks::getStateBlock(key);
                             if(sb != nullptr)
                             {
                                 cout << "      sb: ";
@@ -1114,9 +1107,8 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) c
             if (state_blocks)
             {
                 cout << "    sb:";
-                for (const auto& key : L->getStructure())
+                for (const auto& sb : L->getStateBlockVec())
                 {
-                    const auto& sb = L->getStateBlock(key);
                     if (sb != nullptr)
                         cout << (sb->isFixed() ? " Fix" : " Est");
                 }
@@ -1470,9 +1462,8 @@ bool Problem::check(int verbose_level) const
             cout << (F->isKeyOrAux() ? (F->isKey() ? "  KF" : " EF") : "  F") << F->id() << " @ " << F.get() << endl;
             cout << "    -> P @ " << F->getProblem().get() << endl;
             cout << "    -> T @ " << F->getTrajectory().get() << endl;
-            for (const auto& pair_key_sb : F->getStateBlockMap())
+            for (const auto& sb : F->getStateBlockVec())
             {
-                const auto& sb = pair_key_sb.second;
                 cout <<  "    sb @ " << sb.get();
                 if (sb)
                 {
@@ -1734,9 +1725,8 @@ bool Problem::check(int verbose_level) const
             cout << "  L" << L->id() << " @ " << L.get() << endl;
             cout << "  -> P @ " << L->getProblem().get() << endl;
             cout << "  -> M @ " << L->getMap().get() << endl;
-            for (const auto& key : L->getStructure())
+            for (const auto& sb : L->getStateBlockVec())
             {
-                const auto& sb = L->getStateBlock(key);
                 cout <<  "  sb @ " << sb.get();
                 if (sb)
                 {