implementing, weird error

src/capture/capture_base.cpp

@@ -228,15 +228,12 @@ void CaptureBase::move(FrameBasePtr _frm_ptr)
     WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
     WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");
 
+    assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
+    assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");
+
     // Unlink
     if (this->getFrame())
     {
-        if (this->getFrame()->isKey())
-        {
-            WOLF_ERROR("moving a capture linked to a KF");
-            return;
-        }
-
         // unlink from previous non-key frame
         this->getFrame()->removeCapture(shared_from_this());
         this->setFrame(nullptr);

src/factor/factor_base.cpp

@@ -256,12 +256,17 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
     {
         WOLF_WARN("Linking with nullptr");
         return;
+
+        // if frame, should not be not-key
+        if (getCapture() and getFrame())
+            assert(getFrame()->isKey() && "Forbidden: linking a factor with a non-key frame.");
     }
 
     // link with feature
     _ftr_ptr->addFactor(shared_from_this());
     this->setFeature(_ftr_ptr);
 
+
     // set problem ( and register factor )
     WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
     this->setProblem(_ftr_ptr->getProblem());
@@ -270,7 +275,11 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
     for (auto& frm_ow : frame_other_list_)
     {
         auto frame_other = frm_ow.lock();
-        if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+        if(frame_other != nullptr)
+        {
+            assert(frame_other->isKey() && "Forbidden: linking a factor with a non-key frame_other.");
+            frame_other->addConstrainedBy(shared_from_this());
+        }
     }
     for (auto& cap_ow : capture_other_list_)
     {

test/gtest_factor_autodiff.cpp

@@ -348,8 +348,8 @@ TEST(FactorAutodiff, AutodiffDummy12)
 
 TEST(FactorAutodiff, EmplaceOdom2d)
 {
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1)));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -379,8 +379,8 @@ TEST(FactorAutodiff, ResidualOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -423,8 +423,8 @@ TEST(FactorAutodiff, JacobianOdom2d)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;
@@ -501,8 +501,8 @@ TEST(FactorAutodiff, AutodiffVsAnalytic)
     f1_pose << 2,1,M_PI;
     f2_pose << 3,5,3*M_PI/2;
 
-    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
-    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
+    FrameBasePtr fr1_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f1_pose.head<2>()), std::make_shared<StateBlock>(f1_pose.tail<1>())));
+    FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(KEY, TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>())));
 
     // SENSOR
     ParamsSensorOdom2d intrinsics_odo;

test/gtest_frame_base.cpp

@@ -71,10 +71,15 @@ TEST(FrameBase, LinksToTree)
     auto S = SensorBase::emplace<SensorOdom2d>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
     auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
     auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
 
     //TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
     ASSERT_FALSE(F2->getConstrainedByList().empty());
@@ -91,6 +96,7 @@ TEST(FrameBase, LinksToTree)
     ASSERT_EQ(F1->getHits() , (unsigned int) 0);
 
     // F2 has no capture and one factor-by
+    WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
     ASSERT_TRUE(F2->getCaptureList().empty());
     ASSERT_FALSE(F2->getConstrainedByList().empty());
     ASSERT_EQ(F2->getHits() , (unsigned int) 1);