Skip to content
Snippets Groups Projects
Commit ce184f39 authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

[WIP] removing destruct()

Need to see how to handle StateBlock removal:

Probably use a weak_ptr up, and remove them from Problem lists with
similar remove() methods.

Cannot use the destructor because it may stay alive for any reason:
then, use remove() as we’ll do in all wolf.
parent d26bde85
No related branches found
No related tags found
No related merge requests found
......@@ -79,24 +79,8 @@ ConstraintBase::ConstraintBase(ConstraintType _tp, LandmarkBasePtr _landmark_ptr
ConstraintBase::~ConstraintBase()
{
//std::cout << "deleting ConstraintBase " << nodeId() << std::endl;
is_removing_ = true;
// add constraint to be removed from solver
if (getProblem() != nullptr)
getProblem()->removeConstraintPtr(this);
//std::cout << "removeConstraintPtr " << std::endl;
// remove constraint to frame/landmark/feature
if (frame_other_ptr_ != nullptr)
frame_other_ptr_->removeConstrainedBy(this);
if (feature_other_ptr_ != nullptr)
feature_other_ptr_->removeConstrainedBy(this);
if (landmark_other_ptr_ != nullptr)
landmark_other_ptr_->removeConstrainedBy(this);
//std::cout << "removed constraints to " << std::endl;
std::cout << "destructing c" << id() << std::endl;
}
const Eigen::VectorXs& ConstraintBase::getMeasurement() const
......
......@@ -53,7 +53,6 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
ConstraintBase(ConstraintType _tp, LandmarkBasePtr _landmark_ptr, bool _apply_loss_function, ConstraintStatus _status);
virtual ~ConstraintBase();
void destruct();
void remove();
unsigned int id();
......@@ -160,6 +159,11 @@ inline void ConstraintBase::remove()
if (feature_ptr_->getConstraintListPtr()->empty() && feature_ptr_->getConstrainedByListPtr()->empty())
feature_ptr_->remove(); // remove upstream
// add constraint to be removed from solver
if (getProblem() != nullptr)
getProblem()->removeConstraintPtr(this);
// remove other: {Frame, feature, Landmark}
switch (category_)
{
......@@ -248,17 +252,6 @@ inline FeatureBasePtr ConstraintBase::getFeatureOtherPtr()
return feature_other_ptr_;
}
inline void ConstraintBase::destruct()
{
if (!is_removing_)
{
if (feature_other_ptr_ != nullptr)
feature_other_ptr_->removeConstraint(this);
else
delete this;
}
}
inline LandmarkBasePtr ConstraintBase::getLandmarkOtherPtr()
{
// return landmark_other_ptr_;// TODO remove line
......
......@@ -39,21 +39,22 @@ FeatureBase::FeatureBase(FeatureType _tp, const std::string& _type, const Eigen:
FeatureBase::~FeatureBase()
{
//std::cout << "deleting FeatureBase " << nodeId() << std::endl;
is_removing_ = true;
while (!constrained_by_list_.empty())
{
//std::cout << "destruct() constraint " << (*constrained_by_list_.begin())->nodeId() << std::endl;
constrained_by_list_.front()->destruct();
//std::cout << "deleted " << std::endl;
}
// is_removing_ = true;
//
// while (!constrained_by_list_.empty())
// {
// //std::cout << "destruct() constraint " << (*constrained_by_list_.begin())->nodeId() << std::endl;
// constrained_by_list_.front()->remove();
// //std::cout << "deleted " << std::endl;
// }
//std::cout << "constraints deleted" << std::endl;
while (!constraint_list_.empty())
{
delete constraint_list_.front();
constraint_list_.pop_front();
}
// while (!constraint_list_.empty())
// {
// delete constraint_list_.front();
// constraint_list_.pop_front();
// }
std::cout << "destructed f" << id() << std::endl;
}
......
......@@ -13,34 +13,22 @@ HardwareBase::HardwareBase() :
HardwareBase::~HardwareBase()
{
is_removing_ = true;
while (!sensor_list_.empty())
{
// delete sensor_list_.front(); // TODO remove line
sensor_list_.pop_front();
}
//std::cout << "deleting HardwareBase " << nodeId() << std::endl;
std::cout << "destructing H" << nodeId() << std::endl;
}
SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
//std::cout << "adding sensor..." << std::endl;
sensor_list_.push_back(_sensor_ptr);
_sensor_ptr->setProblem(getProblem());
// addDownNode(_sensor_ptr);
//std::cout << "added!" << std::endl;
_sensor_ptr->registerNewStateBlocks();
return _sensor_ptr;
}
void HardwareBase::removeSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.remove(_sensor_ptr);
// delete _sensor_ptr; // TODO remove line
}
} // namespace wolf
......@@ -22,18 +22,7 @@ class HardwareBase : public NodeBase //: public NodeLinked<Problem, SensorBase>
public:
HardwareBase();
/** \brief Default destructor (not recommended)
*
* Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
*
**/
virtual ~HardwareBase();
void destruct()
{
if (!is_removing_)
delete this;
}
ProblemPtr getProblem(){return problem_ptr_;}
void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
......
......@@ -27,20 +27,6 @@ LandmarkBase::~LandmarkBase()
//std::cout << "deleting LandmarkBase " << nodeId() << std::endl;
is_removing_ = true;
// Remove Frame State Blocks
if (p_ptr_ != nullptr)
{
if (getProblem() != nullptr)
getProblem()->removeStateBlockPtr(p_ptr_);
delete p_ptr_;
}
if (o_ptr_ != nullptr)
{
if (getProblem() != nullptr)
getProblem()->removeStateBlockPtr(o_ptr_);
delete o_ptr_;
}
//std::cout << "states deleted" << std::endl;
while (!constrained_by_list_.empty())
{
......
......@@ -55,7 +55,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
**/
LandmarkBase(const LandmarkType & _tp, const std::string& _type, StateBlock* _p_ptr, StateBlock* _o_ptr = nullptr);
virtual ~LandmarkBase();
void destruct();
// void destruct();
void remove();
/** \brief Returns landmark_id_, the landmark unique id
......@@ -114,6 +114,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
#include "map_base.h"
#include "constraint_base.h"
#include "state_block.h"
namespace wolf{
......@@ -124,9 +125,27 @@ inline void LandmarkBase::remove()
is_removing_ = true;
std::cout << "Removing L" << id() << std::endl;
LandmarkBasePtr this_L = shared_from_this(); // keep this alive while removing it
map_ptr_->getLandmarkListPtr()->remove(shared_from_this()); // remove from upstream
// Remove State Blocks
if (p_ptr_ != nullptr)
{
if (getProblem() != nullptr)
getProblem()->removeStateBlockPtr(p_ptr_);
delete p_ptr_;
}
if (o_ptr_ != nullptr)
{
if (getProblem() != nullptr)
getProblem()->removeStateBlockPtr(o_ptr_);
delete o_ptr_;
}
// remove from upstream
map_ptr_->getLandmarkListPtr()->remove(shared_from_this());
// remove constrained by
while (!constrained_by_list_.empty())
constrained_by_list_.front()->remove(); // remove constrained
constrained_by_list_.front()->remove();
}
}
......@@ -180,7 +199,7 @@ inline void LandmarkBase::removeConstrainedBy(ConstraintBasePtr _ctr_ptr)
{
constrained_by_list_.remove(_ctr_ptr);
if (constrained_by_list_.empty())
this->destruct();
this->remove();
}
inline StateBlock* LandmarkBase::getPPtr() const
......@@ -233,22 +252,22 @@ inline const Eigen::VectorXs& LandmarkBase::getDescriptor() const
return descriptor_;
}
inline void LandmarkBase::destruct()
{
if (!is_removing_)
{
if (map_ptr_ != nullptr) // && !up_node_ptr_->isTop())
{
//std::cout << "upper node is not WolfProblem " << std::endl;
map_ptr_->removeLandmark(shared_from_this());
}
else
{
//std::cout << "upper node is WolfProblem or nullptr" << std::endl;
// delete this;//TODO remove line
}
}
}
//inline void LandmarkBase::destruct()
//{
// if (!is_removing_)
// {
// if (map_ptr_ != nullptr) // && !up_node_ptr_->isTop())
// {
// //std::cout << "upper node is not WolfProblem " << std::endl;
// map_ptr_->removeLandmark(shared_from_this());
// }
// else
// {
// //std::cout << "upper node is WolfProblem or nullptr" << std::endl;
// // delete this;//TODO remove line
// }
// }
//}
inline const LandmarkType LandmarkBase::getTypeId() const
{
......
......@@ -22,19 +22,19 @@ MapBase::MapBase() :
NodeBase("MAP"),
problem_ptr_(nullptr)
{
//std::cout << "MapBase::MapBase(): " << __LINE__ << std::endl;
std::cout << "created M"<< std::endl;
}
MapBase::~MapBase()
{
is_removing_ = true;
while (!landmark_list_.empty())
{
delete landmark_list_.front();
landmark_list_.pop_front();
}
//std::cout << "deleting MapBase " << nodeId() << std::endl;
// is_removing_ = true;
// while (!landmark_list_.empty())
// {
// delete landmark_list_.front();
// landmark_list_.pop_front();
// }
//
std::cout << "destructed M" << nodeId() << std::endl;
}
LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
......
......@@ -31,7 +31,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
* Default destructor (please use destruct() instead of delete for guaranteeing the wolf tree integrity)
**/
~MapBase();
void destruct();
// void destruct();
ProblemPtr getProblem(){return problem_ptr_;}
void setProblem(ProblemPtr _prob_ptr){problem_ptr_ = _prob_ptr;}
......@@ -49,11 +49,11 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
std::string dateTimeNow();
};
inline void MapBase::destruct()
{
if (!is_removing_)
delete this;
}
//inline void MapBase::destruct()
//{
// if (!is_removing_)
// delete this;
//}
inline LandmarkBaseList* MapBase::getLandmarkListPtr()
{
......
......@@ -43,8 +43,15 @@ SensorBase::SensorBase(const SensorType & _tp, const std::string& _type, StateBl
}
SensorBase::~SensorBase()
{
std::cout << "destructed S" << id() << std::endl;
}
inline void SensorBase::remove()
{
is_removing_ = true;
SensorBasePtr this_sen = shared_from_this(); // protect it while removing links
// Remove State Blocks
if (p_ptr_ != nullptr && !extrinsic_dynamic_)
{
......@@ -67,15 +74,17 @@ SensorBase::~SensorBase()
delete intrinsic_ptr_;
}
// remove downstream processors
while (!processor_list_.empty())
{
delete processor_list_.front();
processor_list_.pop_front();
processor_list_.front()->remove();
// delete processor_list_.front();
// processor_list_.pop_front();
}
}
void SensorBase::fix()
{
// State Blocks
......
......@@ -91,7 +91,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
*
**/
virtual ~SensorBase();
void destruct();
// void destruct();
void remove();
unsigned int id();
......@@ -201,17 +202,17 @@ inline Eigen::VectorXs SensorBase::getNoiseStd()
return noise_std_;
}
inline void SensorBase::destruct()
{
if (!is_removing_)
{
if (hardware_ptr_ != nullptr)
// hardware_ptr_->removeSensor(this); // TODO remove line
hardware_ptr_->removeSensor(shared_from_this());
else
delete this;
}
}
//inline void SensorBase::destruct()
//{
// if (!is_removing_)
// {
// if (hardware_ptr_ != nullptr)
// // hardware_ptr_->removeSensor(this); // TODO remove line
// hardware_ptr_->removeSensor(shared_from_this());
// else
// delete this;
// }
//}
inline Eigen::MatrixXs SensorBase::getNoiseCov()
{
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment