diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index 7864c46b9e03eb2b3609ca588d39e4ef1a06d975..a8d5d971fb728f56d5fbf2704a1aa37c86621651 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -19,6 +19,8 @@ namespace wolf{
//class CaptureBase
class CaptureBase : public NodeBase, public std::enable_shared_from_this<CaptureBase>
{
+ friend FactorBase;
+
private:
FrameBaseWPtr frame_ptr_;
FeatureBasePtrList feature_list_;
@@ -61,10 +63,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void unlinkFromFrame(){frame_ptr_.reset();}
virtual void setProblem(ProblemPtr _problem) final;
-
- FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
FeatureBasePtrList& getFeatureList();
- void addFeatureList(FeatureBasePtrList& _new_ft_list);
void getFactorList(FactorBasePtrList& _fac_list);
@@ -106,6 +105,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
protected:
SizeEigen computeCalibSize() const;
+ FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
private:
void updateCalibSize();
};
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 72a2be8a46ba1f4e95b448570ac649d478a86054..69d4d4507781106c3b035d485a5b18e1b6b41655 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -18,6 +18,8 @@ namespace wolf {
//class FeatureBase
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
+ friend FactorBase;
+
private:
CaptureBaseWPtr capture_ptr_;
FactorBasePtrList factor_list_;
@@ -85,7 +87,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
CaptureBasePtr getCapture() const;
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
- FactorBasePtr addFactor(FactorBasePtr _co_ptr);
FactorBasePtrList& getFactorList();
virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
@@ -99,8 +100,11 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
template<typename classType, typename... T>
static std::shared_ptr<FeatureBase> emplace(CaptureBasePtr _cpt_ptr, T&&... all);
- private:
+
+ protected:
+
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+ FactorBasePtr addFactor(FactorBasePtr _co_ptr);
};
}
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 44e1cfbd2981f6d7afe67311c249b9fc67897638..b713fb93b93f3ed68c8c543f5b19b4abf9d8a23f 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -30,6 +30,9 @@ typedef enum
//class FrameBase
class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase>
{
+ friend CaptureBase;
+ friend FactorBase;
+
private:
TrajectoryBaseWPtr trajectory_ptr_;
CaptureBasePtrList capture_list_;
@@ -133,7 +136,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
FrameBasePtr getNextFrame() const;
CaptureBasePtrList& getCaptureList();
- CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
@@ -150,6 +152,9 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
template<typename classType, typename... T>
static std::shared_ptr<FrameBase> emplace(TrajectoryBasePtr _ptr, T&&... all);
+ protected:
+
+ CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
public:
static FrameBasePtr create_PO_2D (const FrameType & _tp,
const TimeStamp& _ts,
diff --git a/include/core/hardware/hardware_base.h b/include/core/hardware/hardware_base.h
index 1b4149a6d32671492d1eee3c05e13dacb79e92fa..07c116e818bb59361098d7f39d0a832996751235 100644
--- a/include/core/hardware/hardware_base.h
+++ b/include/core/hardware/hardware_base.h
@@ -16,6 +16,8 @@ namespace wolf {
//class HardwareBase
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
+ friend SensorBase;
+
private:
SensorBasePtrList sensor_list_;
@@ -23,8 +25,10 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
HardwareBase();
virtual ~HardwareBase();
- virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
SensorBasePtrList& getSensorList();
+
+ protected:
+ virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
};
} // namespace wolf
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 839ee0f97c9694da18cff81b17e78ce853b7e934..a772f69dd6c250d35757d17bbea0b4c018312dac 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -44,8 +44,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
* \param _o_ptr StateBlock pointer to the orientation (default: nullptr)
*
**/
- LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
- LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+ LandmarkBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
+ LandmarkBase(MapBaseWPtr _ptr, const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr);
virtual ~LandmarkBase();
virtual void remove();
virtual YAML::Node saveToYaml() const;
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index 90ad785458a9a3a62f6882c7294945c19ceedcc8..ee6d245283c625fd4f16538620c7b14868ce5850 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -19,6 +19,8 @@ namespace wolf {
//class MapBase
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
+ friend LandmarkBase;
+
private:
LandmarkBasePtrList landmark_list_;
@@ -26,8 +28,10 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
MapBase();
~MapBase();
+ protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
- virtual void addLandmarkList(LandmarkBasePtrList& _landmark_list);
+
+ public:
LandmarkBasePtrList& getLandmarkList();
void load(const std::string& _map_file_yaml);
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2e05d2fe57dfb3494cd3b33c2d697510ff493849..0f1678fd17cdd02aa8935c721ba3ceb48f5071c1 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -29,6 +29,9 @@ struct IntrinsicsBase: public ParamsBase
class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase>
{
+ friend Problem;
+ friend ProcessorBase;
+
private:
HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
@@ -95,9 +98,11 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual void setProblem(ProblemPtr _problem) final;
HardwareBasePtr getHardware();
+ protected:
void setHardware(const HardwareBasePtr _hw_ptr);
-
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
+
+ public:
ProcessorBasePtrList& getProcessorList();
CaptureBasePtr lastKeyCapture(void);
diff --git a/include/core/trajectory/trajectory_base.h b/include/core/trajectory/trajectory_base.h
index 0ee9c37d53052a3da7f193b8a4e6715592d5628b..339a41b3dad5a1cab9743360a9f7e2bd6f55c380 100644
--- a/include/core/trajectory/trajectory_base.h
+++ b/include/core/trajectory/trajectory_base.h
@@ -20,6 +20,8 @@ namespace wolf {
//class TrajectoryBase
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
+ friend FrameBase;
+
private:
std::list<FrameBasePtr> frame_list_;
@@ -47,6 +49,10 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
void sortFrame(FrameBasePtr _frm_ptr);
void updateLastFrames();
+ protected:
+ FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
+
+ public:
// factors
void getFactorList(FactorBasePtrList & _fac_list);
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 78904aa8baac91d47d2300454b8a77b2f1160001..d95ef15d9d37e50f7a9b7cdf53e8baf5ad67c73a 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -102,16 +102,7 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
return _ft_ptr;
}
-void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
{
- for (FeatureBasePtr feature_ptr : _new_ft_list)
- {
- feature_ptr->setCapture(shared_from_this());
- if (getProblem() != nullptr)
- feature_ptr->setProblem(getProblem());
- // feature_list_.push_back(feature_ptr);
- }
- feature_list_.splice(feature_list_.end(), _new_ft_list);
}
void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)