diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index a7e0235490ba9212a766f414f5b72539c93347f8..aa942ce6c8468924775c74b6763eb796f0cfaed7 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -78,15 +78,10 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
friend FeatureBase;
private:
FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
+ ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
static unsigned int factor_id_count_;
- protected:
- unsigned int factor_id_;
- FactorTopology topology_; ///< topology of factor
- FactorStatus status_; ///< status of factor
- bool apply_loss_function_; ///< flag for applying loss function to this factor
-
FrameBaseWPtrList frame_other_list_; ///< FrameBase pointer list
CaptureBaseWPtrList capture_other_list_; ///< CaptureBase pointer list
FeatureBaseWPtrList feature_other_list_; ///< FeatureBase pointer list
@@ -96,8 +91,12 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
CaptureBaseConstWPtrList capture_other_const_list_; ///< CaptureBase pointer list
FeatureBaseConstWPtrList feature_other_const_list_; ///< FeatureBase pointer list
LandmarkBaseConstWPtrList landmark_other_const_list_; ///< LandmarkBase pointer list
-
- ProcessorBaseWPtr processor_ptr_; ///< Processor pointer
+
+ protected:
+ unsigned int factor_id_;
+ FactorTopology topology_; ///< topology of factor
+ FactorStatus status_; ///< status of factor
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
Eigen::VectorXd measurement_; ///< the measurement vector
Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
@@ -133,8 +132,6 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
bool _apply_loss_function,
FactorStatus _status = FAC_ACTIVE);
-
-
~FactorBase() override = default;
virtual void remove(bool viral_remove_empty_parent=false);
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 2ab79c9ee19c6122a0521afd0e7f5a456fd255a5..df548a3a55d0d46138ff425a6cf7e281cedd86fb 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -118,12 +118,12 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_)
+ CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
+
protected:
Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise
- CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree)
-
void setProblem(ProblemPtr _problem) override final;
public:
@@ -229,8 +229,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
StateBlockPtr getIntrinsic();
protected:
- void removeStateBlocks();
- virtual void registerNewStateBlocks();
+ virtual void registerNewStateBlocks(ProblemPtr _problem) override;
public:
diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h
index 3bec157836352109d794012456549c19412c411f..ccfbc51485f904932cf8a6fadb5ea9be31585b09 100644
--- a/include/core/state_block/has_state_blocks.h
+++ b/include/core/state_block/has_state_blocks.h
@@ -85,8 +85,8 @@ class HasStateBlocks
StateBlockPtr emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_base_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
- void registerNewStateBlocks(ProblemPtr _problem);
- void removeStateBlocks(ProblemPtr _problem);
+ virtual void registerNewStateBlocks(ProblemPtr _problem);
+ virtual void removeStateBlocks(ProblemPtr _problem);
// States
VectorComposite getState(const StateStructure& structure="") const;
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index a1a8331a94f07128619f6215587459d2640c8d5a..891a319de0bc62e43aa3493164be9f41168f8d30 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -39,10 +39,7 @@ FactorBase::FactorBase(const std::string& _tp,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
+ processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
@@ -51,7 +48,10 @@ FactorBase::FactorBase(const std::string& _tp,
capture_other_const_list_(),
feature_other_const_list_(),
landmark_other_const_list_(),
- processor_ptr_(_processor_ptr)
+ factor_id_(++factor_id_count_),
+ topology_(_top),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function)
{
if (_frame_other_ptr)
{
@@ -91,10 +91,7 @@ FactorBase::FactorBase(const std::string& _tp,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
- factor_id_(++factor_id_count_),
- topology_(_top),
- status_(_status),
- apply_loss_function_(_apply_loss_function),
+ processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
@@ -103,7 +100,10 @@ FactorBase::FactorBase(const std::string& _tp,
capture_other_const_list_(),
feature_other_const_list_(),
landmark_other_const_list_(),
- processor_ptr_(_processor_ptr)
+ factor_id_(++factor_id_count_),
+ topology_(_top),
+ status_(_status),
+ apply_loss_function_(_apply_loss_function)
{
for (auto& Fo : _frame_other_list)
{
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index c6027ce19a092b86b4eb6d5e0223ae1edbec5641..f18d627aa37dec917ecbbf77c982294724a6b589 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -42,9 +42,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_size),
- noise_cov_(_noise_size, _noise_size),
- last_capture_(nullptr)
+ noise_cov_(_noise_size, _noise_size)
{
assert((_p_ptr or not _p_dyn) and "Trying to set dynamic position state block without providing a position state block. It is required anyway.");
assert((_o_ptr or not _o_dyn) and "Trying to set dynamic orientation state block without providing an orientation state block. It is required anyway.");
@@ -76,9 +78,11 @@ SensorBase::SensorBase(const std::string& _type,
HasStateBlocks(""),
hardware_ptr_(),
sensor_id_(++sensor_id_count_), // simple ID factory
+ state_block_dynamic_(),
+ params_prior_map_(),
+ last_capture_(nullptr),
noise_std_(_noise_std),
- noise_cov_(_noise_std.size(), _noise_std.size()),
- last_capture_(nullptr)
+ noise_cov_(_noise_std.size(), _noise_std.size())
{
setNoiseStd(_noise_std);
@@ -95,23 +99,7 @@ SensorBase::SensorBase(const std::string& _type,
SensorBase::~SensorBase()
{
// Remove State Blocks
- removeStateBlocks();
-}
-
-void SensorBase::removeStateBlocks()
-{
- for (const auto& key : getStructure())
- {
- auto sbp = getStateBlock(key);
- if (sbp != nullptr)
- {
- if (getProblem() != nullptr && !isStateBlockInCapture(key))
- {
- getProblem()->notifyStateBlock(sbp,REMOVE);
- }
- removeStateBlock(key);
- }
- }
+ removeStateBlocks(getProblem());
}
void SensorBase::fixExtrinsics()
@@ -203,17 +191,17 @@ void SensorBase::addPriorParameter(const char& _key, const Eigen::VectorXd& _x,
params_prior_map_[_key] = ftr_prior;
}
-void SensorBase::registerNewStateBlocks()
+void SensorBase::registerNewStateBlocks(ProblemPtr _problem)
{
- if (getProblem() != nullptr)
+ if (_problem != nullptr)
{
for (auto & pair_key_sbp : getStateBlockMap())
{
auto key = pair_key_sbp.first;
auto sbp = pair_key_sbp.second;
- if (sbp and not isStateBlockDynamic(key))//!isStateBlockInCapture(key))
- getProblem()->notifyStateBlock(sbp, ADD);
+ if (sbp and not isStateBlockDynamic(key))
+ _problem->notifyStateBlock(sbp, ADD);
}
}
}
@@ -532,7 +520,7 @@ void SensorBase::link(HardwareBasePtr _hw_ptr)
this->setHardware(_hw_ptr);
this->getHardware()->addSensor(shared_from_this());
this->setProblem(_hw_ptr->getProblem());
- this->registerNewStateBlocks();
+ this->registerNewStateBlocks(getProblem());
}
else
{