diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 9f548fd325de45d8901f432975bf23563938c988..64a4741c518ecd0f75bf029aa4e3267b1e3f8c4e 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -195,7 +195,11 @@ struct MatrixSizeCheck
typedef std::list<ClassName##Ptr> ClassName##PtrList; \
typedef ClassName##PtrList::iterator ClassName##Iter; \
typedef ClassName##PtrList::const_iterator ClassName##ConstIter; \
- typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter; \
+ typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
+ typedef ClassName##WPtrList::iterator ClassName##WIter; \
+ typedef ClassName##WPtrList::const_iterator ClassName##WConstIter; \
+ typedef ClassName##WPtrList::reverse_iterator ClassName##WRevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 6f215396071b757b980faac31be8d0250716ea71..8ecf4fe494f6f2714cc7492718a521dcf2ef1629 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -40,19 +40,19 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
{
friend FeatureBase;
private:
- FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
static unsigned int factor_id_count_;
protected:
unsigned int factor_id_;
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer
- CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer
- ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+ FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
+ CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
+ FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
+ LandmarkBaseWPtrList landmark_other_list_; ///< LandmarkBase pointer list
+ ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer list
virtual void setProblem(ProblemPtr) final;
public:
@@ -72,6 +72,17 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
+ FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status = FAC_ACTIVE);
+
+
+
virtual ~FactorBase() = default;
virtual void remove(bool viral_remove_empty_parent=false);
@@ -149,21 +160,32 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
*/
bool getApplyLossFunction() const;
- /** \brief Returns a pointer to the frame constrained to
+ /** \brief Returns a pointer to the first frame constrained to
**/
- FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
+ FrameBasePtr getFrameOther() const;
- /** \brief Returns a pointer to the capture constrained to
+ /** \brief Returns a pointer to the first capture constrained to
**/
- CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
+ CaptureBasePtr getCaptureOther() const;
- /** \brief Returns a pointer to the feature constrained to
+ /** \brief Returns a pointer to the first feature constrained to
**/
- FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
+ FeatureBasePtr getFeatureOther() const;
- /** \brief Returns a pointer to the landmark constrained to
+ /** \brief Returns a pointer to the first landmark constrained to
**/
- LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
+ LandmarkBasePtr getLandmarkOther() const;
+
+ // get pointer lists to other nodes
+ FrameBaseWPtrList getFrameOtherList() const { return frame_other_list_; }
+ CaptureBaseWPtrList getCaptureOtherList() const { return capture_other_list_; }
+ FeatureBaseWPtrList getFeatureOtherList() const { return feature_other_list_; }
+ LandmarkBaseWPtrList getLandmarkOtherList() const { return landmark_other_list_; }
+
+ bool hasFrameOther(const FrameBasePtr& _frm_other);
+ bool hasCaptureOther(const CaptureBasePtr& _cap_other);
+ bool hasFeatureOther(const FeatureBasePtr& _ftr_other);
+ bool hasLandmarkOther(const LandmarkBasePtr& _lmk_other);
public:
/**
@@ -228,6 +250,39 @@ inline bool FactorBase::getApplyLossFunction() const
return apply_loss_function_;
}
+inline FrameBasePtr FactorBase::getFrameOther() const
+{
+ if (frame_other_list_.empty()) return nullptr;
+ if (frame_other_list_.front().expired()) return nullptr;
+
+ return frame_other_list_.front().lock();
+}
+
+inline CaptureBasePtr FactorBase::getCaptureOther() const
+{
+ if (capture_other_list_.empty()) return nullptr;
+ if (capture_other_list_.front().expired()) return nullptr;
+
+ return capture_other_list_.front().lock();
+}
+
+inline FeatureBasePtr FactorBase::getFeatureOther() const
+{
+ if (feature_other_list_.empty()) return nullptr;
+ if (feature_other_list_.front().expired()) return nullptr;
+
+ return feature_other_list_.front().lock();
+}
+
+inline LandmarkBasePtr FactorBase::getLandmarkOther() const
+{
+ if (landmark_other_list_.empty()) return nullptr;
+ if (landmark_other_list_.front().expired()) return nullptr;
+
+ return landmark_other_list_.front().lock();
+}
+
+
inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 46b244d9e83f9c01f4fca0bf84af35d81d36d53f..8e39c60db29d77fb7349d33f0ee02aa2d0d2a3c6 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -19,15 +19,52 @@ FactorBase::FactorBase(const std::string& _tp,
factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
- frame_other_ptr_(_frame_other_ptr),
- capture_other_ptr_(_capture_other_ptr),
- feature_other_ptr_(_feature_other_ptr),
- landmark_other_ptr_(_landmark_other_ptr),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
processor_ptr_(_processor_ptr)
{
-// std::cout << "constructed +c" << id() << std::endl;
+ if (_frame_other_ptr)
+ frame_other_list_.push_back(_frame_other_ptr);
+ if (_capture_other_ptr)
+ capture_other_list_.push_back(_capture_other_ptr);
+ if (_feature_other_ptr)
+ feature_other_list_.push_back(_feature_other_ptr);
+ if (_landmark_other_ptr)
+ landmark_other_list_.push_back(_landmark_other_ptr);
}
+FactorBase::FactorBase(const std::string& _tp,
+ const FrameBasePtrList& _frame_other_list,
+ const CaptureBasePtrList& _capture_other_list,
+ const FeatureBasePtrList& _feature_other_list,
+ const LandmarkBasePtrList& _landmark_other_list,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ NodeBase("FACTOR", _tp),
+ feature_ptr_(),
+ factor_id_(++factor_id_count_),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function),
+ frame_other_list_(),
+ capture_other_list_(),
+ feature_other_list_(),
+ landmark_other_list_(),
+ processor_ptr_(_processor_ptr)
+{
+ for (auto& Fo : _frame_other_list)
+ frame_other_list_.push_back(Fo);
+ for (auto& Co : _capture_other_list)
+ capture_other_list_.push_back(Co);
+ for (auto& fo : _feature_other_list)
+ feature_other_list_.push_back(fo);
+ for (auto& Lo : landmark_other_list_)
+ landmark_other_list_.push_back(Lo);
+}
+
+
void FactorBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
@@ -46,36 +83,48 @@ void FactorBase::remove(bool viral_remove_empty_parent)
getProblem()->notifyFactor(this_fac,REMOVE);
// remove other: {Frame, Capture, Feature, Landmark}
- FrameBasePtr frm_o = frame_other_ptr_.lock();
- if (frm_o)
+ for (auto& frm_ow : frame_other_list_)
{
- frm_o->removeConstrainedBy(this_fac);
- if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
- frm_o->remove(viral_remove_empty_parent);
+ auto frm_o = frm_ow.lock();
+ if (frm_o)
+ {
+ frm_o->removeConstrainedBy(this_fac);
+ if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
+ frm_o->remove(viral_remove_empty_parent);
+ }
}
- CaptureBasePtr cap_o = capture_other_ptr_.lock();
- if (cap_o)
+ for (auto& cap_ow : capture_other_list_)
{
- cap_o->removeConstrainedBy(this_fac);
- if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
- cap_o->remove(viral_remove_empty_parent);
+ auto cap_o = cap_ow.lock();
+ if (cap_o)
+ {
+ cap_o->removeConstrainedBy(this_fac);
+ if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
+ cap_o->remove(viral_remove_empty_parent);
+ }
}
- FeatureBasePtr ftr_o = feature_other_ptr_.lock();
- if (ftr_o)
+ for (auto& ftr_ow : feature_other_list_)
{
- ftr_o->removeConstrainedBy(this_fac);
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
- ftr_o->remove(viral_remove_empty_parent);
+ auto ftr_o = ftr_ow.lock();
+ if (ftr_o)
+ {
+ ftr_o->removeConstrainedBy(this_fac);
+ if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
+ ftr_o->remove(viral_remove_empty_parent);
+ }
}
- LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
- if (lmk_o)
+ for (auto& lmk_ow : landmark_other_list_)
{
- lmk_o->removeConstrainedBy(this_fac);
- if (lmk_o->getConstrainedByList().empty())
- lmk_o->remove(viral_remove_empty_parent);
+ auto lmk_o = lmk_ow.lock();
+ if (lmk_o)
+ {
+ lmk_o->removeConstrainedBy(this_fac);
+ if (lmk_o->getConstrainedByList().empty())
+ lmk_o->remove(viral_remove_empty_parent);
+ }
}
// std::cout << "Removed c" << id() << std::endl;
@@ -120,6 +169,66 @@ void FactorBase::setStatus(FactorStatus _status)
status_ = _status;
}
+bool FactorBase::hasFrameOther(const FrameBasePtr &_frm_other)
+{
+ FrameBaseWPtrList::iterator frm_it = find_if(frame_other_list_.begin(),
+ frame_other_list_.end(),
+ [_frm_other](const FrameBaseWPtr &frm_ow)
+ {
+ return frm_ow.lock() == _frm_other;
+ }
+ );
+ if (frm_it != frame_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasCaptureOther(const CaptureBasePtr &_cap_other)
+{
+ CaptureBaseWPtrList::iterator cap_it = find_if(capture_other_list_.begin(),
+ capture_other_list_.end(),
+ [_cap_other](const CaptureBaseWPtr &cap_ow)
+ {
+ return cap_ow.lock() == _cap_other;
+ }
+ );
+ if (cap_it != capture_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasFeatureOther(const FeatureBasePtr &_ftr_other)
+{
+ FeatureBaseWPtrList::iterator ftr_it = find_if(feature_other_list_.begin(),
+ feature_other_list_.end(),
+ [_ftr_other](const FeatureBaseWPtr &ftr_ow)
+ {
+ return ftr_ow.lock() == _ftr_other;
+ }
+ );
+ if (ftr_it != feature_other_list_.end())
+ return true;
+
+ return false;
+}
+
+bool FactorBase::hasLandmarkOther(const LandmarkBasePtr &_lmk_other)
+{
+ LandmarkBaseWPtrList::iterator lmk_it = find_if(landmark_other_list_.begin(),
+ landmark_other_list_.end(),
+ [_lmk_other](const LandmarkBaseWPtr &lmk_ow)
+ {
+ return lmk_ow.lock() == _lmk_other;
+ }
+ );
+ if (lmk_it != landmark_other_list_.end())
+ return true;
+
+ return false;
+}
+
//void FactorBase::setApplyLossFunction(const bool _apply)
//{
// apply_loss_function_ = _apply;
@@ -146,14 +255,26 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
this->setProblem(_ftr_ptr->getProblem());
// constrained by
- auto frame_other = this->frame_other_ptr_.lock();
- if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
- auto capture_other = this->capture_other_ptr_.lock();
- if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
- auto feature_other = this->feature_other_ptr_.lock();
- if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
- auto landmark_other = this->landmark_other_ptr_.lock();
- if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ for (auto& frm_ow : frame_other_list_)
+ {
+ auto frame_other = frm_ow.lock();
+ if(frame_other != nullptr) frame_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& cap_ow : capture_other_list_)
+ {
+ auto capture_other = cap_ow.lock();
+ if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& ftr_ow : feature_other_list_)
+ {
+ auto feature_other = ftr_ow.lock();
+ if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
+ }
+ for (auto& lmk_ow : landmark_other_list_)
+ {
+ auto landmark_other = lmk_ow.lock();
+ if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
+ }
}
void FactorBase::setProblem(ProblemPtr _problem)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index c79f3130d87ba3f82f416a93e31b5946b6e2044a..691aac3904e75cd44b7b03a8c9051ee8f3e34b0d 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1503,14 +1503,17 @@ bool Problem::check(int verbose_level) const
// check problem and trajectory pointers
is_consistent = is_consistent && (F->getProblem().get() == P_raw);
is_consistent = is_consistent && (F->getTrajectory() == T);
+ // check constrained_by
for (auto cby : F->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
+ cout << " <- c" << cby->id() << " -> ";
+ for (const auto& Fow : cby->getFrameOtherList())
+ cout << " F" << Fow.lock()->id() << endl;
}
// check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOther() == F);
+ is_consistent = is_consistent && (cby->hasFrameOther(F));
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose_level > 0)
@@ -1554,6 +1557,35 @@ bool Problem::check(int verbose_level) const
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
is_consistent = is_consistent && (C->getFrame() == F);
+ // check contrained_by
+ for (const auto& cby : C->getConstrainedByList())
+ {
+ if (verbose_level > 0)
+ {
+ cout << " <- c" << cby->id() << " -> ";
+ for (const auto& Cow : cby->getCaptureOtherList())
+ cout << " C" << Cow.lock()->id();
+ cout << endl;
+ }
+ // check constrained_by pointer to this capture
+ is_consistent = is_consistent && (cby->hasCaptureOther(C));
+ for (auto sb : cby->getStateBlockPtrVector())
+ {
+ if (verbose_level > 0)
+ {
+ cout << " sb @ " << sb.get();
+ if (sb)
+ {
+ auto lp = sb->getLocalParametrization();
+ if (lp)
+ cout << " (lp @ " << lp.get() << ")";
+ }
+ cout << endl;
+ }
+ }
+
+ }
+
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
@@ -1568,14 +1600,18 @@ bool Problem::check(int verbose_level) const
is_consistent = is_consistent && (f->getProblem().get() == P_raw);
is_consistent = is_consistent && (f->getCapture() == C);
+ // check contrained_by
for (auto cby : f->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
+ cout << " <- c" << cby->id() << " -> ";
+ for (const auto& fow : cby->getFeatureOtherList())
+ cout << " f" << fow.lock()->id();
+ cout << endl;
}
// check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOther() == f);
+ is_consistent = is_consistent && (cby->hasFeatureOther(f));
}
// Factors =======================================================================================