Merge branch 'capture_remove' into 'master'

Capture remove

See merge request mobile_robotics/wolf!173

src/capture_base.cpp

@@ -90,6 +90,10 @@ void CaptureBase::remove()
         {
             feature_list_.front()->remove(); // remove downstream
         }
+        while (!constrained_by_list_.empty())
+        {
+            constrained_by_list_.front()->remove(); // remove constrained by
+        }
     }
 }
 
@@ -146,7 +150,7 @@ void CaptureBase::removeStateBlocks()
 {
     for (unsigned int i = 0; i < state_block_vec_.size(); i++)
     {
-        auto sbp = getStateBlockPtr(i);
+        auto sbp = state_block_vec_[i];
         if (sbp != nullptr)
         {
             if (getProblem() != nullptr)

src/constraint_base.cpp

@@ -62,7 +62,7 @@ void ConstraintBase::remove()
         if (getProblem() != nullptr)
             getProblem()->removeConstraintPtr(shared_from_this());
 
-        // remove other: {Frame, feature, Landmark}
+        // remove other: {Frame, Capture, Feature, Landmark}
         FrameBasePtr frm_o = frame_other_ptr_.lock();
         if (frm_o)
         {
@@ -71,6 +71,14 @@ void ConstraintBase::remove()
                 frm_o->remove();
         }
 
+        CaptureBasePtr cap_o = capture_other_ptr_.lock();
+        if (cap_o)
+        {
+            cap_o->getConstrainedByList().remove(shared_from_this());
+            if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+                cap_o->remove();
+        }
+
         FeatureBasePtr ftr_o = feature_other_ptr_.lock();
         if (ftr_o)
         {

src/feature_base.cpp

@@ -30,6 +30,8 @@ void FeatureBase::remove()
     {
         is_removing_ = true;
         FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
+
+        // remove from upstream
         CaptureBasePtr C = capture_ptr_.lock();
         if (C)
         {
@@ -37,6 +39,8 @@ void FeatureBase::remove()
             if (C->getFeatureList().empty())
                 C->remove(); // remove upstream
         }
+
+        // remove downstream
         while (!constraint_list_.empty())
         {
             constraint_list_.front()->remove(); // remove downstream
@@ -45,7 +49,6 @@ void FeatureBase::remove()
         {
             constrained_by_list_.front()->remove(); // remove constrained
         }
-//        std::cout << "Removed           f" << id() << std::endl;
     }
 }
 

src/frame_base.cpp

@@ -69,12 +69,15 @@ void FrameBase::remove()
     {
         is_removing_ = true;
         FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
+
+        // remove from upstream
         TrajectoryBasePtr T = trajectory_ptr_.lock();
         if (T)
         {
             T->getFrameList().remove(this_F); // remove from upstream
         }
 
+        // remove downstream
         while (!capture_list_.empty())
         {
             capture_list_.front()->remove(); // remove downstream

src/problem.cpp

@@ -697,7 +697,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
     cout << "Hardware" << ((depth < 1) ? ("   -- " + std::to_string(getHardwarePtr()->getSensorList().size()) + "S") : "")  << endl;
     if (depth >= 1)
     {
-        // Sensors
+        // Sensors =======================================================================================
         for (auto S : getHardwarePtr()->getSensorList())
         {
             cout << "  S" << S->id() << " " << S->getType();
@@ -742,7 +742,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
             }
             if (depth >= 2)
             {
-                // Processors
+                // Processors =======================================================================================
                 for (auto p : S->getProcessorList())
                 {
                     if (p->isMotion())
@@ -781,7 +781,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
     cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectoryPtr()->getFrameList().size()) + "F") : "")  << endl;
     if (depth >= 1)
     {
-        // Frames
+        // Frames =======================================================================================
         for (auto F : getTrajectoryPtr()->getFrameList())
         {
             cout << (F->isKey() ? "  KF" : "  F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C  " : "");
@@ -809,7 +809,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
             }
             if (depth >= 2)
             {
-                // Captures
+                // Captures =======================================================================================
                 for (auto C : F->getCaptureList())
                 {
                     cout << "    C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
@@ -865,7 +865,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
 
                     if (depth >= 3)
                     {
-                        // Features
+                        // Features =======================================================================================
                         for (auto f : C->getFeatureList())
                         {
                             cout << "      f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c  " : "");
@@ -881,7 +881,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
                                         << " )" << endl;
                             if (depth >= 4)
                             {
-                                // Constraints
+                                // Constraints =======================================================================================
                                 for (auto c : f->getConstraintList())
                                 {
                                     cout << "        c" << c->id() << " " << c->getType() << " -->";
@@ -907,7 +907,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
     cout << "Map" << ((depth < 1) ? ("        -- " + std::to_string(getMapPtr()->getLandmarkList().size()) + "L") : "") << endl;
     if (depth >= 1)
     {
-        // Landmarks
+        // Landmarks =======================================================================================
         for (auto L : getMapPtr()->getLandmarkList())
         {
             cout << "  L" << L->id() << " " << L->getType();
@@ -967,6 +967,8 @@ bool Problem::check(int verbose_level)
     }
     // check pointer to Problem
     is_consistent = is_consistent && (H->getProblem().get() == P_raw);
+
+    // Sensors =======================================================================================
     for (auto S : H->getSensorList())
     {
         if (verbose_level > 0)
@@ -989,6 +991,8 @@ bool Problem::check(int verbose_level)
         // check problem and hardware pointers
         is_consistent = is_consistent && (S->getProblem().get() == P_raw);
         is_consistent = is_consistent && (S->getHardwarePtr() == H);
+
+        // Processors =======================================================================================
         for (auto p : S->getProcessorList())
         {
             if (verbose_level > 0)
@@ -1012,6 +1016,8 @@ bool Problem::check(int verbose_level)
     }
     // check pointer to Problem
     is_consistent = is_consistent && (T->getProblem().get() == P_raw);
+
+    // Frames =======================================================================================
     for (auto F : T->getFrameList())
     {
         if (verbose_level > 0)
@@ -1057,6 +1063,8 @@ bool Problem::check(int verbose_level)
                 }
             }
         }
+
+        // Captures =======================================================================================
         for (auto C : F->getCaptureList())
         {
             if (verbose_level > 0)
@@ -1082,6 +1090,8 @@ bool Problem::check(int verbose_level)
             // check problem and frame pointers
             is_consistent = is_consistent && (C->getProblem().get() == P_raw);
             is_consistent = is_consistent && (C->getFramePtr() == F);
+
+            // Features =======================================================================================
             for (auto f : C->getFeatureList())
             {
                 if (verbose_level > 0)
@@ -1104,6 +1114,8 @@ bool Problem::check(int verbose_level)
                     // check constrained_by pointer to this feature
                     is_consistent = is_consistent && (cby->getFeatureOtherPtr() == f);
                 }
+
+                // Constraints =======================================================================================
                 for (auto c : f->getConstraintList())
                 {
                     if (verbose_level > 0)
@@ -1267,6 +1279,8 @@ bool Problem::check(int verbose_level)
         cout << "M @ " << M.get() << endl;
     // check pointer to Problem
     is_consistent = is_consistent && (M->getProblem().get() == P_raw);
+
+    // Landmarks =======================================================================================
     for (auto L : M->getLandmarkList())
     {
         if (verbose_level > 0)