From 9fbd496d0b060a5d7b55b184d4331794e8647957 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Wed, 17 Jul 2019 15:05:20 +0200
Subject: [PATCH] Setting up setProblem as protected and adding as friends base
classes;
---
include/core/capture/capture_base.h | 3 ++-
include/core/common/node_base.h | 3 ++-
include/core/factor/factor_base.h | 3 ++-
include/core/feature/feature_base.h | 6 ++++--
include/core/frame/frame_base.h | 2 +-
include/core/landmark/landmark_base.h | 4 ++--
include/core/processor/processor_base.h | 1 +
include/core/processor/processor_motion.h | 2 +-
include/core/sensor/sensor_base.h | 3 ++-
src/feature/feature_base.cpp | 4 ++--
10 files changed, 19 insertions(+), 12 deletions(-)
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index f5c7a657f..acecc0f7c 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 69d8145ea..59b5123c4 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 a6890dca2..b6b795394 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 2e3719dd6..eb4094d1a 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 853b8f64c..189b5b5a0 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 ce1ab0860..d4843590b 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 b2027b3d3..c7eb875a9 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 15f0ca900..30db66d5a 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 dcd736654..fb7444439 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 f2b4dfca9..97ef674f4 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
--
GitLab