Improve check()

src/problem/problem.cpp

@@ -1873,75 +1873,94 @@ bool Problem::check(int verbose_level) const
                         std::vector<StateBlockPtr> sb_vec;
 
                         // find in own Frame
-                        sb_vec = F->getStateBlockVec();
-                    	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        sb_vec 		= F->getStateBlockVec();
+                    	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                         if (found_here) cout << " F" << F->id();
-                        found = found || found_here;
+                        found 		= found || found_here;
 
                         // find in own Capture
-                        sb_vec = C->getStateBlockVec();
-                    	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        sb_vec 		= C->getStateBlockVec();
+                    	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                         if (found_here) cout << " C" << C->id();
-                        found = found || found_here;
+                        found 		= found || found_here;
+
+                        // Find in other Captures of the own Frame
+                        if (!found_here)
+                        	for (auto FC : F->getCaptureList())
+                        	{
+                        		sb_vec 		= FC->getStateBlockVec();
+                        		found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        		if (found_here) cout << " FC" << FC->id();
+                        		found 		= found || found_here;
+                        	}
 
                         // find in own Sensor
                         if (S)
                         {
-                            sb_vec = S->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                            sb_vec 		= S->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " S" << S->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
                         }
 
                         // find in constrained Frame
                         if (Fo){
-                        	sb_vec = Fo->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        	sb_vec 		= Fo->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                         	if (found_here) cout << " Fo" << Fo->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
+
+                            // Find in Captures of the constrained Frame
+                            for (auto FoC : Fo->getCaptureList())
+                            {
+                            	sb_vec 		= FoC->getStateBlockVec();
+                            	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                            	if (found_here) cout << " FoC" << FoC->id();
+                            	found 		= found || found_here;
+                            }
                         }
 
                         // find in constrained Capture
                         if (Co)
                         {
-                        	sb_vec = Co->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        	sb_vec 		= Co->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " Co" << Co->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
                         }
 
                         // find in constrained Feature
                         if (fo)
                         {
                             // find in constrained feature's Frame
-                            FrameBasePtr foF = fo->getFrame();
-                            sb_vec = foF->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                            auto foF 	= fo->getFrame();
+                            sb_vec 		= foF->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " foF" << foF->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
 
                             // find in constrained feature's Capture
-                            CaptureBasePtr foC = fo->getCapture();
-                            sb_vec = foC->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                            auto foC 	= fo->getCapture();
+                            sb_vec 		= foC->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " foC" << foC->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
 
                             // find in constrained feature's Sensor
-                            SensorBasePtr foS = fo->getCapture()->getSensor();
-                            sb_vec = foS->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                            auto foS 	= fo->getCapture()->getSensor();
+                            sb_vec 		= foS->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " foS" << foS->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
                         }
 
                         // find in constrained landmark
                         if (Lo)
                         {
-                        	sb_vec = Lo->getStateBlockVec();
-                        	found_here = (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
+                        	sb_vec 		= Lo->getStateBlockVec();
+                        	found_here 	= (std::find(sb_vec.begin(), sb_vec.end(), sb) != sb_vec.end());
                             if (found_here) cout << " Lo" << Lo->id();
-                            found = found || found_here;
+                            found 		= found || found_here;
                         }
 
                         if (verbose_level > 0)