Skip to content
Snippets Groups Projects
Commit 755c8622 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

constrained_by add/remove protected and const ref getter

parent 6ef15933
No related branches found
No related tags found
1 merge request!274Resolve "Emplace API inconsistent with ProcessorTrackerFeature/Landmark functions"
This commit is part of merge request !274. Comments created here will be created in the context of that merge request.
Hide whitespace changes
Inline Side-by-side
Showing
with 91 additions and 23 deletions
include/core/capture/capture_base.h
+ 6
− 2
View file @ 755c8622
......@@ -19,6 +19,7 @@ namespace wolf{
//class CaptureBase
class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
{
friend FeatureBase;
friend FactorBase;
private:
......@@ -71,9 +72,12 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
protected:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
public:
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
const FactorBasePtrList& getConstrainedByList() const;
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
......@@ -199,7 +203,7 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
inline FactorBasePtrList& CaptureBase::getConstrainedByList()
inline const FactorBasePtrList& CaptureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
......
include/core/factor/factor_base.h
+ 7
− 5
View file @ 755c8622
......@@ -142,23 +142,25 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the frame constrained to
**/
FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
/** \brief Returns a pointer to the frame constrained to
/** \brief Returns a pointer to the capture constrained to
**/
CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
// void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
// void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
// void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
// void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
public:
/**
* @brief getProcessor
* @return
......
include/core/feature/feature_base.h
+ 4
− 3
View file @ 755c8622
......@@ -89,9 +89,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FactorBasePtrList& getFactorList();
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
const FactorBasePtrList& getConstrainedByList() const;
// all factors
void getFactorList(FactorBasePtrList & _fac_list);
......@@ -105,6 +104,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
......@@ -128,7 +129,7 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
inline FactorBasePtrList& FeatureBase::getConstrainedByList()
inline const FactorBasePtrList& FeatureBase::getConstrainedByList() const
{
return constrained_by_list_;
}
......
include/core/frame/frame_base.h
+ 4
− 3
View file @ 755c8622
......@@ -145,9 +145,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
void getFactorList(FactorBasePtrList& _fac_list);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
const FactorBasePtrList& getConstrainedByList() const;
void link(TrajectoryBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
......@@ -165,6 +164,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
static FrameBasePtr create_POV_3D(const FrameType & _tp,
const TimeStamp& _ts,
const Eigen::VectorXs& _x = Eigen::VectorXs::Zero(10));
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
} // namespace wolf
......@@ -294,7 +295,7 @@ inline void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_problem);
}
inline FactorBasePtrList& FrameBase::getConstrainedByList()
inline const FactorBasePtrList& FrameBase::getConstrainedByList() const
{
return constrained_by_list_;
}
......
include/core/landmark/landmark_base.h
+ 8
− 3
View file @ 755c8622
......@@ -22,6 +22,8 @@ namespace wolf {
//class LandmarkBase
class LandmarkBase : public NodeBase, public std::enable_shared_from_this<LandmarkBase>
{
friend FactorBase;
private:
MapBaseWPtr map_ptr_;
FactorBasePtrList constrained_by_list_;
......@@ -86,9 +88,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
FactorBasePtrList& getConstrainedByList();
const FactorBasePtrList& getConstrainedByList() const;
void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
......@@ -96,6 +97,10 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
template<typename classType, typename... T>
static std::shared_ptr<LandmarkBase> emplace(MapBasePtr _map_ptr, T&&... all);
protected:
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
}
......@@ -145,7 +150,7 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
inline const FactorBasePtrList& LandmarkBase::getConstrainedByList() const
{
return constrained_by_list_;
}
......
src/capture/capture_base.cpp
+ 5
− 0
View file @ 755c8622
......@@ -118,6 +118,11 @@ FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
return _fac_ptr;
}
void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
}
StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
{
if (getSensor())
......
src/factor/factor_base.cpp
+ 42
− 4
View file @ 755c8622
......@@ -64,7 +64,7 @@ void FactorBase::remove()
FrameBasePtr frm_o = frame_other_ptr_.lock();
if (frm_o)
{
frm_o->getConstrainedByList().remove(shared_from_this());
frm_o->removeConstrainedBy(shared_from_this());
if (frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove();
}
......@@ -72,7 +72,7 @@ void FactorBase::remove()
CaptureBasePtr cap_o = capture_other_ptr_.lock();
if (cap_o)
{
cap_o->getConstrainedByList().remove(shared_from_this());
cap_o->removeConstrainedBy(shared_from_this());
if (cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove();
}
......@@ -80,7 +80,7 @@ void FactorBase::remove()
FeatureBasePtr ftr_o = feature_other_ptr_.lock();
if (ftr_o)
{
ftr_o->getConstrainedByList().remove(shared_from_this());
ftr_o->removeConstrainedBy(shared_from_this());
if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove();
}
......@@ -88,7 +88,7 @@ void FactorBase::remove()
LandmarkBasePtr lmk_o = landmark_other_ptr_.lock();
if (lmk_o)
{
lmk_o->getConstrainedByList().remove(shared_from_this());
lmk_o->removeConstrainedBy(shared_from_this());
if (lmk_o->getConstrainedByList().empty())
lmk_o->remove();
}
......@@ -174,4 +174,42 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
auto landmark_other = this->landmark_other_ptr_.lock();
if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
}
//void FactorBase::setFrameOther(FrameBasePtr _frm_o)
//{
// if (_frm_o)
// {
// frame_other_ptr_ = _frm_o;
// frame_other_ptr_->addConstrainedBy(shared_from_this());
// }
//}
//
//void FactorBase::setCaptureOther(CaptureBasePtr _cap_o)
//{
// if (_cap_o)
// {
// capture_other_ptr_ = _cap_o;
// frame_other_ptr_->addConstrainedBy(shared_from_this());
// }
//}
//
//void FactorBase::setFeatureOther(FeatureBasePtr _ftr_o)
//{
// if (_ftr_o)
// {
// feature_other_ptr_ = _ftr_o;
// frame_other_ptr_->addConstrainedBy(shared_from_this());
// }
//}
//
//void FactorBase::setLandmarkOther(LandmarkBasePtr _lmk_o)
//{
// if (_lmk_o)
// {
// landmark_other_ptr_ = _lmk_o;
// frame_other_ptr_->addConstrainedBy(shared_from_this());
// }
//}
} // namespace wolf
src/feature/feature_base.cpp
+ 5
− 1
View file @ 755c8622
......@@ -78,10 +78,14 @@ FrameBasePtr FeatureBase::getFrame() const
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
_fac_ptr->setFeatureOther(shared_from_this());
return _fac_ptr;
}
void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
}
FactorBasePtrList& FeatureBase::getFactorList()
{
return factor_list_;
......
src/frame/frame_base.cpp
+ 5
− 1
View file @ 755c8622
......@@ -379,10 +379,14 @@ void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
_fac_ptr->setFrameOther(shared_from_this());
return _fac_ptr;
}
void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
}
FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
const TimeStamp& _ts,
const Eigen::VectorXs& _x)
......
src/landmark/landmark_base.cpp
+ 5
− 1
View file @ 755c8622
......@@ -188,8 +188,12 @@ void LandmarkBase::link(MapBasePtr _map_ptr)
FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
_fac_ptr->setLandmarkOther(shared_from_this());
return _fac_ptr;
}
void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
}
} // namespace wolf
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment