From 9266743bc05e4d250af33a30c5888337c663755e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Mon, 27 May 2019 11:13:22 +0200
Subject: [PATCH] added and or protected removeChild() and setParent() and
removed addXXXList()
---
include/core/capture/capture_base.h | 1 +
include/core/factor/factor_base.h | 4 +++-
include/core/feature/feature_base.h | 4 +++-
include/core/frame/frame_base.h | 3 ++-
include/core/landmark/landmark_base.h | 2 +-
include/core/map/map_base.h | 1 +
include/core/processor/processor_base.h | 2 ++
include/core/sensor/sensor_base.h | 2 ++
include/core/trajectory/trajectory_base.h | 2 +-
src/capture/capture_base.cpp | 3 ++-
src/feature/feature_base.cpp | 5 +++++
src/frame/frame_base.cpp | 5 +++++
src/map/map_base.cpp | 15 ++-------------
src/problem/problem.cpp | 10 ----------
src/sensor/sensor_base.cpp | 5 +++++
src/trajectory/trajectory_base.cpp | 10 ++++++++++
16 files changed, 45 insertions(+), 29 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 08751287b..8732820b4 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -110,6 +110,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
SizeEigen computeCalibSize() const;
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
+ void removeFeature(FeatureBasePtr _ft_ptr);
private:
void updateCalibSize();
};
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 796579ce3..b4ea5b7c7 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -113,7 +113,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a pointer to the feature constrained from
**/
FeatureBasePtr getFeature() const;
- void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
@@ -177,6 +176,9 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
template<typename classType, typename... T>
static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
+ protected:
+
+ void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
// protected:
// template<typename D>
// void print(const std::string& name, const Eigen::MatrixBase<D>& mat) const; // Do nothing if input Scalar type is ceres::Jet
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index c3a709fd5..5e4b6cc9d 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -85,7 +85,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
FrameBasePtr getFrame() const;
CaptureBasePtr getCapture() const;
- void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtrList& getFactorList();
@@ -103,7 +102,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
protected:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+
+ void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ void removeFactor(FactorBasePtr _co_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 98f50bb77..bc0591388 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -130,7 +130,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
virtual void setProblem(ProblemPtr _problem) final;
TrajectoryBasePtr getTrajectory() const;
- void setTrajectory(TrajectoryBasePtr _trj_ptr);
FrameBasePtr getPreviousFrame() const;
FrameBasePtr getNextFrame() const;
@@ -154,6 +153,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
protected:
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
+ void removeCapture(CaptureBasePtr _capt_ptr);
+ void setTrajectory(TrajectoryBasePtr _trj_ptr);
public:
static FrameBasePtr create_PO_2D (const FrameType & _tp,
const TimeStamp& _ts,
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 5c27689da..fc8b87c08 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -91,7 +91,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
- void setMap(const MapBasePtr _map_ptr);
MapBasePtr getMap();
void link(MapBasePtr);
template<typename classType, typename... T>
@@ -99,6 +98,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
protected:
+ void setMap(const MapBasePtr _map_ptr);
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
virtual void removeConstrainedBy(FactorBasePtr _fac_ptr);
};
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index ee6d24528..136f42f80 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -30,6 +30,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
+ virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
public:
LandmarkBasePtrList& getLandmarkList();
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index a6f8ac588..7c171a11f 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -204,8 +204,10 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
SensorBasePtr getSensor();
const SensorBasePtr getSensor() const;
+ protected:
void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
+ public:
virtual bool isMotion();
void setTimeTolerance(Scalar _time_tolerance);
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 0f1678fd1..010d3fb78 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -101,6 +101,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
protected:
void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
+ void removeProcessor(ProcessorBasePtr _proc_ptr);
public:
ProcessorBasePtrList& getProcessorList();
@@ -186,6 +187,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
Eigen::MatrixXs getNoiseCov();
void setExtrinsicDynamic(bool _extrinsic_dynamic);
void setIntrinsicDynamic(bool _intrinsic_dynamic);
+
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<SensorBase> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 339a41b3d..5d2216379 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -38,7 +38,6 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
std::string getFrameStructure() const;
// Frames
- FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
FrameBasePtrList& getFrameList();
const FrameBasePtrList& getFrameList() const;
FrameBasePtr getLastFrame() const;
@@ -51,6 +50,7 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
protected:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
+ void removeFrame(FrameBasePtr _frame_ptr);
public:
// factors
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 5aa5ea366..f531a781e 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -102,7 +102,9 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
return _ft_ptr;
}
+void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
+ feature_list_.remove(_ft_ptr);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
@@ -114,7 +116,6 @@ void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
- _fac_ptr->setCaptureOther(shared_from_this());
return _fac_ptr;
}
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 26b325da8..33ee54e3d 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -70,6 +70,11 @@ FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
return _co_ptr;
}
+void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
+{
+ factor_list_.remove(_co_ptr);
+}
+
FrameBasePtr FeatureBase::getFrame() const
{
return capture_ptr_.lock()->getFrame();
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 7fd6bf365..02914eb1a 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -323,6 +323,11 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
return _capt_ptr;
}
+void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
+{
+ capture_list_.remove(_capt_ptr);
+}
+
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
diff --git a/src/map/map_base.cpp b/src/map/map_base.cpp
index 204fc7b8f..0da491faa 100644
--- a/src/map/map_base.cpp
+++ b/src/map/map_base.cpp
@@ -33,20 +33,9 @@ LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
return _landmark_ptr;
}
-void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
+void MapBase::removeLandmark(LandmarkBasePtr _landmark_ptr)
{
- for (auto lmk : _landmark_list)
- addLandmark(lmk);
-
- //TEMPORARY FIX, should be made compliant with the new emplace methodology
- LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
- for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
- {
- landmark_ptr->setMap(shared_from_this());
- landmark_ptr->setProblem(getProblem());
- landmark_ptr->registerNewStateBlocks();
- }
- landmark_list_.splice(landmark_list_.end(), _landmark_list);
+ landmark_list_.remove(_landmark_ptr);
}
void MapBase::load(const std::string& _map_file_dot_yaml)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 87819e750..455c72b99 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -430,16 +430,6 @@ void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _proces
processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
-LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
-{
- getMap()->addLandmark(_lmk_ptr);
- return _lmk_ptr;
-}
-
-void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
-{
- getMap()->addLandmarkList(_lmk_list);
-}
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
{
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c633261ad..4986a3521 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -339,6 +339,11 @@ ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
return _proc_ptr;
}
+void SensorBase::removeProcessor(ProcessorBasePtr _proc_ptr)
+{
+ processor_list_.remove(_proc_ptr);
+}
+
StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
{
CaptureBasePtr cap;
diff --git a/src/trajectory/trajectory_base.cpp b/src/trajectory/trajectory_base.cpp
index a2395ffae..34921e3f4 100644
--- a/src/trajectory/trajectory_base.cpp
+++ b/src/trajectory/trajectory_base.cpp
@@ -34,6 +34,16 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
return _frame_ptr;
}
+void TrajectoryBase::removeFrame(FrameBasePtr _frame_ptr)
+{
+ // add to list
+ frame_list_.remove(_frame_ptr);
+
+ // update last_estimated and last_key
+ if (_frame_ptr->isKeyOrAux())
+ updateLastFrames();
+}
+
void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_ptr : getFrameList())
--
GitLab