diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 9d4b96c3b9a096089b9ad051b4cf8a48510860c1..65a78728260a755e3b9ddbc370d7d8bc623a3160 100644
--- a/src/capture/capture_base.cpp
+++ b/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);
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 0f65ddb4dcd0587fff4cc9cae0d0f6491c10f437..ee45aa68570db221c37b2d686c26aab948d6f40e 100644
--- a/src/factor/factor_base.cpp
+++ b/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_)
{
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index b2b28b32d89412e3afbd2f3466946bed6cc1932b..68712f969a298f5986b93378480f1a66e03cafe1 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/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;
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 90ba45c62add57f2f5d222579810d39f69b0cf6b..64ff216fb064e3038cda35cf0c391552fadeace1 100644
--- a/test/gtest_frame_base.cpp
+++ b/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);