diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index f5c7a657f2305ae3d9f540865ef6c26b81f8ec8f..acecc0f7c6a9aecd4de40a34e178b08654f46dc5 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -21,6 +21,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
{
friend FeatureBase;
friend FactorBase;
+ friend FrameBase;
private:
FrameBaseWPtr frame_ptr_;
@@ -36,6 +37,7 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
protected:
unsigned int capture_id_;
TimeStamp time_stamp_; ///< Time stamp
+ virtual void setProblem(ProblemPtr _problem) final;
public:
@@ -64,7 +66,6 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void setFrame(const FrameBasePtr _frm_ptr);
public:
- virtual void setProblem(ProblemPtr _problem) final;
const FeatureBasePtrList& getFeatureList() const;
void getFactorList(FactorBasePtrList& _fac_list);
diff --git a/include/core/common/node_base.h b/include/core/common/node_base.h
index 69d8145eaa5b5a591fae3f57c3e2954df93f9f62..59b5123c4971be28214e50021fe1fff0dc21d537 100644
--- a/include/core/common/node_base.h
+++ b/include/core/common/node_base.h
@@ -53,6 +53,7 @@ namespace wolf {
class NodeBase
{
+ friend Problem;
private:
static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
@@ -68,6 +69,7 @@ class NodeBase
bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
+ virtual void setProblem(ProblemPtr _prob_ptr);
public:
NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
@@ -82,7 +84,6 @@ class NodeBase
void setType(const std::string& _type);
void setName(const std::string& _name);
ProblemPtr getProblem() const;
- virtual void setProblem(ProblemPtr _prob_ptr);
};
} // namespace wolf
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index a6890dca220baa11ce1b33d5a85f17290a3e3626..b6b7953940279fef26735a06b3d012910590be77 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -38,6 +38,7 @@ typedef enum
//class FactorBase
class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBase>
{
+ friend FeatureBase;
private:
FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
@@ -53,6 +54,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer
ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
+ virtual void setProblem(ProblemPtr) final;
public:
/** \brief Constructor not involving nodes of other frames, only feature, capture and/or frame of this factor
@@ -162,7 +164,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
ProcessorBasePtr getProcessor() const;
void link(FeatureBasePtr ftr);
- virtual void setProblem(ProblemPtr) final;
template<typename classType, typename... T>
static std::shared_ptr<FactorBase> emplace(FeatureBasePtr _oth_ptr, T&&... all);
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index 2e3719dd6e58b9b7f473c66779ce4326b3747278..eb4094d1aa0581478aeef801845abd8ee98ca1b1 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -19,6 +19,8 @@ namespace wolf {
class FeatureBase : public NodeBase, public std::enable_shared_from_this<FeatureBase>
{
friend FactorBase;
+ friend CaptureBase;
+ friend SensorBase;
private:
CaptureBaseWPtr capture_ptr_;
@@ -35,7 +37,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
Eigen::MatrixXs measurement_sqrt_information_upper_; ///< the squared root information matrix
Eigen::VectorXs expectation_; ///< expectation
-
+ virtual void setProblem(ProblemPtr _problem) final;
+
public:
typedef enum
@@ -55,7 +58,6 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
virtual ~FeatureBase();
virtual void remove(bool viral_remove_empty_parent=false);
- virtual void setProblem(ProblemPtr _problem) final;
// properties
unsigned int id();
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index 853b8f64c8883fb307128d714d79b62d7c0a8905..189b5b5a06ceb0275fa1d11c59ff25a8dad4bc89 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -112,6 +112,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
protected:
void registerNewStateBlocks();
void removeStateBlocks();
+ virtual void setProblem(ProblemPtr _problem) final;
// Fixed / Estimated
public:
@@ -129,7 +130,6 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
// Wolf tree access ---------------------------------------------------
public:
- virtual void setProblem(ProblemPtr _problem) final;
TrajectoryBasePtr getTrajectory() const;
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index ce1ab0860d1767b396a302f7f8671aa09498e22a..d4843590bd3974d02bcded1360616bac13cf8adb 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -32,6 +32,8 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
static unsigned int landmark_id_count_;
protected:
+ // Navigate wolf tree
+ virtual void setProblem(ProblemPtr _problem) final;
unsigned int landmark_id_; ///< landmark unique id
TimeStamp stamp_; ///< stamp of the creation of the landmark
Eigen::VectorXs descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
@@ -84,8 +86,6 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
Scalar getDescriptor(unsigned int _ii) const;
void setDescriptor(const Eigen::VectorXs& _descriptor);
- // Navigate wolf tree
- virtual void setProblem(ProblemPtr _problem) final;
unsigned int getHits() const;
const FactorBasePtrList& getConstrainedByList() const;
diff --git a/include/core/processor/processor_base.h b/include/core/processor/processor_base.h
index b2027b3d3b07d41af584043e69ddf7abd9ecfa80..c7eb875a9732732b5193ab684eaa63648920d60f 100644
--- a/include/core/processor/processor_base.h
+++ b/include/core/processor/processor_base.h
@@ -204,6 +204,7 @@ struct ProcessorParamsBase : public ParamsBase
//class ProcessorBase
class ProcessorBase : public NodeBase, public std::enable_shared_from_this<ProcessorBase>
{
+ friend SensorBase;
protected:
unsigned int processor_id_;
ProcessorParamsBasePtr params_;
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 15f0ca9004ffc8ca36efadf233eb9a0bfbca384d..30db66d5a2a0fd2bb2b65cb4f51a51ea460d08af 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -140,6 +140,7 @@ class ProcessorMotion : public ProcessorBase
protected:
ProcessorParamsMotionPtr params_motion_;
ProcessingStep processing_step_; ///< State machine controlling the processing step
+ virtual void setProblem(ProblemPtr) override;
// This is the main public interface
public:
@@ -214,7 +215,6 @@ class ProcessorMotion : public ProcessorBase
*/
FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
- virtual void setProblem(ProblemPtr) override;
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index dcd73665457f2b78e1d9aed0678dabbc63e4da4c..fb7444439d51bb68ef0c27d3a48cd7d860973299 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -57,6 +57,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
+ virtual void setProblem(ProblemPtr _problem) final;
+
public:
/** \brief Constructor with noise size
*
@@ -100,7 +102,6 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
unsigned int id();
- virtual void setProblem(ProblemPtr _problem) final;
HardwareBasePtr getHardware();
private:
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index f2b4dfca9b65bd7cf0ad64bca7de55d8dc53d830..97ef674f4f6cbd73a9cfb9b3e38b5e74c9344739 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -129,8 +129,8 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
void FeatureBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
- for (auto ctr : factor_list_)
- ctr->setProblem(_problem);
+ for (auto fac : factor_list_)
+ fac->setProblem(_problem);
}
Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const