diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index 00d5698016447ede93473c94fc36b2a57179beeb..b12d92f10a430810150040fe9277346d2f63c620 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -326,6 +326,8 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual bool permittedAuxFrame() final;
+ virtual void setProblem(ProblemPtr) override;
+
public:
/**\brief notify a new keyframe made by another processor
*
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 4568c6fb3e04b9f3081e27666bc7e9cf3b22e806..2eebf5410599b6eca26a734884956b80f701448e 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -147,7 +147,6 @@ class ProcessorMotion : public ProcessorBase, public IsMotion
protected:
ParamsProcessorMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
- virtual void setProblem(ProblemPtr) override;
// This is the main public interface
public:
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/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 101bd5abb500ff95feec86e3c8969ab4f82301d5..6f28aef4d3393d1baf4a652fa4a06052ba9abb82 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -104,6 +104,23 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
WOLF_WARN("Linking with a nullptr");
}
}
+
+void ProcessorBase::setProblem(ProblemPtr _problem)
+{
+ std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
+ assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+
+ if (_problem == nullptr or _problem == this->getProblem())
+ return;
+
+ NodeBase::setProblem(_problem);
+
+ // adding processor is motion to the processor is motion vector
+ auto is_motion_ptr = std::dynamic_pointer_cast<IsMotion>(shared_from_this());
+ if (is_motion_ptr)
+ getProblem()->addProcessorIsMotion(is_motion_ptr);
+}
+
/////////////////////////////////////////////////////////////////////////////////////////
void BufferPackKeyFrame::add(const FrameBasePtr& _key_frame, const double& _time_tolerance)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index b08d88dc77683f1018a0e077c58b4bd039e9bdd3..a6cddeff1fc847cdd792106b5a4039ca092410cc 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -691,24 +691,6 @@ PackKeyFramePtr ProcessorMotion::computeProcessingStep()
return nullptr;
}
-void ProcessorMotion::setProblem(ProblemPtr _problem)
-{
- std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
- assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
-
- if (_problem == nullptr or _problem == this->getProblem())
- return;
-
- NodeBase::setProblem(_problem);
-
- // set the origin
- if (origin_ptr_ == nullptr && this->getProblem()->getLastKeyFrame() != nullptr)
- this->setOrigin(this->getProblem()->getLastKeyFrame());
-
- // adding processor is motion to the processor is motion vector
- getProblem()->addProcessorIsMotion(std::dynamic_pointer_cast<IsMotion>(shared_from_this()));
-}
-
bool ProcessorMotion::storeKeyFrame(FrameBasePtr _frame_ptr)
{
return true;
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..b55811471cf6c34dc564ec9f25eafaed9b62da27 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -69,10 +69,15 @@ TEST(FrameBase, LinksToTree)
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
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));
+ auto F1 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
+ auto F2 = FrameBase::emplace<FrameBase>(T, KEY, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto C = CaptureBase::emplace<CaptureMotion>(F1, "CaptureMotion", 1, S, Vector3d::Zero(), 3, 3, nullptr);
- auto p = ProcessorBase::emplace<ProcessorOdom2d>(S, std::make_shared<ParamsProcessorOdom2d>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ auto p = std::make_shared<ProcessorOdom2d>(std::make_shared<ParamsProcessorOdom2d>());
+ WOLF_INFO("F2->getCaptureList().size() ", F2->getCaptureList().size());
+ p->link(S);
+ //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);
auto c = FactorBase::emplace<FactorOdom2d>(f, f, F2, p, false);