Skip to content
Snippets Groups Projects
Commit 7edd01f9 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

Check getTop()!=nullptr after calling methods from getTop()

parent 245820c7
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/frame_base.cpp
+ 51
− 12
View file @ 7edd01f9
...@@ -37,17 +37,20 @@ FrameBase::~FrameBase() ...@@ -37,17 +37,20 @@ FrameBase::~FrameBase()
// Remove Frame State Blocks // Remove Frame State Blocks
if (p_ptr_ != nullptr) if (p_ptr_ != nullptr)
{ {
getTop()->removeStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->removeStateBlockPtr(p_ptr_);
delete p_ptr_; delete p_ptr_;
} }
if (o_ptr_ != nullptr) if (o_ptr_ != nullptr)
{ {
getTop()->removeStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->removeStateBlockPtr(o_ptr_);
delete o_ptr_; delete o_ptr_;
} }
if (v_ptr_ != nullptr) if (v_ptr_ != nullptr)
{ {
getTop()->removeStateBlockPtr(v_ptr_); if (getTop() != nullptr)
getTop()->removeStateBlockPtr(v_ptr_);
delete v_ptr_; delete v_ptr_;
} }
//std::cout << "states deleted" << std::endl; //std::cout << "states deleted" << std::endl;
...@@ -117,7 +120,24 @@ Eigen::VectorXs FrameBase::getState() const ...@@ -117,7 +120,24 @@ Eigen::VectorXs FrameBase::getState() const
Eigen::VectorXs state((p_ptr_==nullptr ? 0 : p_ptr_->getSize()) + Eigen::VectorXs state((p_ptr_==nullptr ? 0 : p_ptr_->getSize()) +
(o_ptr_==nullptr ? 0 : o_ptr_->getSize()) + (o_ptr_==nullptr ? 0 : o_ptr_->getSize()) +
(v_ptr_==nullptr ? 0 : v_ptr_->getSize())); (v_ptr_==nullptr ? 0 : v_ptr_->getSize()));
state << p_ptr_->getVector(), o_ptr_->getVector(), v_ptr_->getVector();
unsigned int index = 0;
if (p_ptr_!=nullptr)
{
state.head(p_ptr_->getSize());
index += p_ptr_->getSize();
}
if (o_ptr_!=nullptr)
{
state.segment(index, o_ptr_->getSize());
index += p_ptr_->getSize();
}
if (v_ptr_!=nullptr)
{
state.segment(index, v_ptr_->getSize());
// index += v_ptr_->getSize();
}
return state; return state;
} }
...@@ -137,7 +157,11 @@ void FrameBase::getConstraintList(ConstraintBaseList & _ctr_list) ...@@ -137,7 +157,11 @@ void FrameBase::getConstraintList(ConstraintBaseList & _ctr_list)
FrameBase* FrameBase::getPreviousFrame() const FrameBase* FrameBase::getPreviousFrame() const
{ {
//std::cout << "finding previous frame of " << this->node_id_ << std::endl; std::cout << "finding previous frame of " << this->node_id_ << std::endl;
if (getTrajectoryPtr() == nullptr)
std::cout << "This Frame is not linked to any trajectory" << std::endl;
assert(getTrajectoryPtr() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory) //look for the position of this node in the upper list (frame list of trajectory)
for (auto f_it = getTrajectoryPtr()->getFrameListPtr()->rbegin(); f_it != getTrajectoryPtr()->getFrameListPtr()->rend(); f_it++ ) for (auto f_it = getTrajectoryPtr()->getFrameListPtr()->rbegin(); f_it != getTrajectoryPtr()->getFrameListPtr()->rend(); f_it++ )
...@@ -145,7 +169,16 @@ FrameBase* FrameBase::getPreviousFrame() const ...@@ -145,7 +169,16 @@ FrameBase* FrameBase::getPreviousFrame() const
if ( this->node_id_ == (*f_it)->nodeId() ) if ( this->node_id_ == (*f_it)->nodeId() )
{ {
f_it++; f_it++;
return *f_it; if (f_it != getTrajectoryPtr()->getFrameListPtr()->rend())
{
std::cout << "previous frame found!" << std::endl;
return *f_it;
}
else
{
std::cout << "previous frame not found!" << std::endl;
return nullptr;
}
} }
} }
std::cout << "previous frame not found!" << std::endl; std::cout << "previous frame not found!" << std::endl;
...@@ -182,17 +215,20 @@ void FrameBase::setStatus(StateStatus _st) ...@@ -182,17 +215,20 @@ void FrameBase::setStatus(StateStatus _st)
if (p_ptr_ != nullptr) if (p_ptr_ != nullptr)
{ {
p_ptr_->fix(); p_ptr_->fix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_ != nullptr) if (o_ptr_ != nullptr)
{ {
o_ptr_->fix(); o_ptr_->fix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
if (v_ptr_ != nullptr) if (v_ptr_ != nullptr)
{ {
v_ptr_->fix(); v_ptr_->fix();
getTop()->updateStateBlockPtr(v_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(v_ptr_);
} }
} }
else if (status_ == ST_ESTIMATED) else if (status_ == ST_ESTIMATED)
...@@ -200,17 +236,20 @@ void FrameBase::setStatus(StateStatus _st) ...@@ -200,17 +236,20 @@ void FrameBase::setStatus(StateStatus _st)
if (p_ptr_ != nullptr) if (p_ptr_ != nullptr)
{ {
p_ptr_->unfix(); p_ptr_->unfix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_ != nullptr) if (o_ptr_ != nullptr)
{ {
o_ptr_->unfix(); o_ptr_->unfix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
if (v_ptr_ != nullptr) if (v_ptr_ != nullptr)
{ {
v_ptr_->unfix(); v_ptr_->unfix();
getTop()->updateStateBlockPtr(v_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(v_ptr_);
} }
} }
} }
src/hardware_base.cpp
+ 8
− 5
View file @ 7edd01f9
...@@ -20,11 +20,14 @@ void HardwareBase::addSensor(SensorBase* _sensor_ptr) ...@@ -20,11 +20,14 @@ void HardwareBase::addSensor(SensorBase* _sensor_ptr)
//std::cout << "added!" << std::endl; //std::cout << "added!" << std::endl;
// Remove Frame State Blocks // Remove Frame State Blocks
if (_sensor_ptr->getPPtr() != nullptr && getTop() != nullptr) if (getTop() != nullptr)
getTop()->addStateBlockPtr(_sensor_ptr->getPPtr()); {
if (_sensor_ptr->getPPtr() != nullptr)
if (_sensor_ptr->getOPtr() != nullptr && getTop() != nullptr) getTop()->addStateBlockPtr(_sensor_ptr->getPPtr());
getTop()->addStateBlockPtr(_sensor_ptr->getOPtr());
if (_sensor_ptr->getOPtr() != nullptr)
getTop()->addStateBlockPtr(_sensor_ptr->getOPtr());
}
} }
......
src/landmark_base.cpp
+ 12
− 6
View file @ 7edd01f9
...@@ -24,12 +24,14 @@ LandmarkBase::~LandmarkBase() ...@@ -24,12 +24,14 @@ LandmarkBase::~LandmarkBase()
// Remove Frame State Blocks // Remove Frame State Blocks
if (p_ptr_ != nullptr) if (p_ptr_ != nullptr)
{ {
getTop()->removeStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->removeStateBlockPtr(p_ptr_);
delete p_ptr_; delete p_ptr_;
} }
if (o_ptr_ != nullptr) if (o_ptr_ != nullptr)
{ {
getTop()->removeStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->removeStateBlockPtr(o_ptr_);
delete o_ptr_; delete o_ptr_;
} }
//std::cout << "states deleted" << std::endl; //std::cout << "states deleted" << std::endl;
...@@ -76,12 +78,14 @@ void LandmarkBase::setStatus(LandmarkStatus _st) ...@@ -76,12 +78,14 @@ void LandmarkBase::setStatus(LandmarkStatus _st)
if (p_ptr_!=nullptr) if (p_ptr_!=nullptr)
{ {
p_ptr_->fix(); p_ptr_->fix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_!=nullptr) if (o_ptr_!=nullptr)
{ {
o_ptr_->fix(); o_ptr_->fix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
} }
else if(status_ == LANDMARK_ESTIMATED) else if(status_ == LANDMARK_ESTIMATED)
...@@ -89,12 +93,14 @@ void LandmarkBase::setStatus(LandmarkStatus _st) ...@@ -89,12 +93,14 @@ void LandmarkBase::setStatus(LandmarkStatus _st)
if (p_ptr_!=nullptr) if (p_ptr_!=nullptr)
{ {
p_ptr_->unfix(); p_ptr_->unfix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_!=nullptr) if (o_ptr_!=nullptr)
{ {
o_ptr_->unfix(); o_ptr_->unfix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
} }
} }
......
src/map_base.cpp
+ 7
− 4
View file @ 7edd01f9
...@@ -17,10 +17,13 @@ void MapBase::addLandmark(LandmarkBase* _landmark_ptr) ...@@ -17,10 +17,13 @@ void MapBase::addLandmark(LandmarkBase* _landmark_ptr)
{ {
addDownNode(_landmark_ptr); addDownNode(_landmark_ptr);
if (_landmark_ptr->getPPtr() != nullptr) if (getTop()!= nullptr)
getTop()->addStateBlockPtr(_landmark_ptr->getPPtr()); {
if (_landmark_ptr->getOPtr() != nullptr) if (_landmark_ptr->getPPtr() != nullptr)
getTop()->addStateBlockPtr(_landmark_ptr->getOPtr()); getTop()->addStateBlockPtr(_landmark_ptr->getPPtr());
if (_landmark_ptr->getOPtr() != nullptr)
getTop()->addStateBlockPtr(_landmark_ptr->getOPtr());
}
} }
void MapBase::removeLandmark(const LandmarkBaseIter& _landmark_iter) void MapBase::removeLandmark(const LandmarkBaseIter& _landmark_iter)
......
src/sensor_base.cpp
+ 8
− 4
View file @ 7edd01f9
...@@ -81,12 +81,14 @@ void SensorBase::fix() ...@@ -81,12 +81,14 @@ void SensorBase::fix()
if (p_ptr_!=nullptr) if (p_ptr_!=nullptr)
{ {
p_ptr_->fix(); p_ptr_->fix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_!=nullptr) if (o_ptr_!=nullptr)
{ {
o_ptr_->fix(); o_ptr_->fix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
} }
...@@ -96,12 +98,14 @@ void SensorBase::unfix() ...@@ -96,12 +98,14 @@ void SensorBase::unfix()
if (p_ptr_!=nullptr) if (p_ptr_!=nullptr)
{ {
p_ptr_->unfix(); p_ptr_->unfix();
getTop()->updateStateBlockPtr(p_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(p_ptr_);
} }
if (o_ptr_!=nullptr) if (o_ptr_!=nullptr)
{ {
o_ptr_->unfix(); o_ptr_->unfix();
getTop()->updateStateBlockPtr(o_ptr_); if (getTop() != nullptr)
getTop()->updateStateBlockPtr(o_ptr_);
} }
} }
......
src/trajectory_base.cpp
+ 2
− 1
View file @ 7edd01f9
...@@ -18,7 +18,8 @@ TrajectoryBase::~TrajectoryBase() ...@@ -18,7 +18,8 @@ TrajectoryBase::~TrajectoryBase()
void TrajectoryBase::addFrame(FrameBase* _frame_ptr) void TrajectoryBase::addFrame(FrameBase* _frame_ptr)
{ {
addDownNode(_frame_ptr); addDownNode(_frame_ptr);
if (getTop() != nullptr){ if (getTop() != nullptr)
{
if (_frame_ptr->getPPtr() != nullptr) if (_frame_ptr->getPPtr() != nullptr)
getTop()->addStateBlockPtr(_frame_ptr->getPPtr()); getTop()->addStateBlockPtr(_frame_ptr->getPPtr());
if (_frame_ptr->getOPtr() != nullptr) if (_frame_ptr->getOPtr() != nullptr)
......
src/wolf_manager.cpp
+ 17
− 10
View file @ 7edd01f9
...@@ -54,7 +54,7 @@ WolfManager::~WolfManager() ...@@ -54,7 +54,7 @@ WolfManager::~WolfManager()
void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp) void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp)
{ {
//std::cout << "creating new frame..." << std::endl; std::cout << "creating new frame..." << std::endl;
// current frame -> KEYFRAME // current frame -> KEYFRAME
last_key_frame_ = current_frame_; last_key_frame_ = current_frame_;
...@@ -66,7 +66,7 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta ...@@ -66,7 +66,7 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta
// Store new current frame // Store new current frame
current_frame_ = problem_->getLastFramePtr(); current_frame_ = problem_->getLastFramePtr();
//std::cout << "current_frame_" << std::endl; std::cout << "current_frame_" << std::endl;
// Zero odometry (to be integrated) // Zero odometry (to be integrated)
if (last_key_frame_ != nullptr) if (last_key_frame_ != nullptr)
...@@ -76,7 +76,7 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta ...@@ -76,7 +76,7 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta
empty_odom->process(); empty_odom->process();
last_capture_relative_ = empty_odom; last_capture_relative_ = empty_odom;
} }
//std::cout << "last_key_frame_" << std::endl; std::cout << "last_key_frame_" << std::endl;
// ---------------------- KEY FRAME --------------------- // ---------------------- KEY FRAME ---------------------
if (last_key_frame_ != nullptr) if (last_key_frame_ != nullptr)
...@@ -91,17 +91,17 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta ...@@ -91,17 +91,17 @@ void WolfManager::createFrame(const Eigen::VectorXs& _frame_state, const TimeSta
} }
//std::cout << "Last key frame non-odometry captures processed" << std::endl; std::cout << "Last key frame non-odometry captures processed" << std::endl;
// ---------------------- MANAGE WINDOW OF POSES --------------------- // ---------------------- MANAGE WINDOW OF POSES ---------------------
manageWindow(); manageWindow();
//std::cout << "new frame created" << std::endl; std::cout << "new frame created" << std::endl;
} }
void WolfManager::createFrame(const TimeStamp& _time_stamp) void WolfManager::createFrame(const TimeStamp& _time_stamp)
{ {
//std::cout << "creating new frame from prior..." << std::endl; std::cout << "creating new frame from prior..." << std::endl;
createFrame(last_capture_relative_->computeFramePose(_time_stamp), _time_stamp); createFrame(last_capture_relative_->computeFramePose(_time_stamp), _time_stamp);
} }
...@@ -147,7 +147,7 @@ bool WolfManager::checkNewFrame(CaptureBase* new_capture) ...@@ -147,7 +147,7 @@ bool WolfManager::checkNewFrame(CaptureBase* new_capture)
void WolfManager::update() void WolfManager::update()
{ {
//std::cout << "updating..." << std::endl; std::cout << "updating..." << std::endl;
while (!new_captures_.empty()) while (!new_captures_.empty())
{ {
// EXTRACT NEW CAPTURE // EXTRACT NEW CAPTURE
...@@ -168,17 +168,19 @@ void WolfManager::update() ...@@ -168,17 +168,19 @@ void WolfManager::update()
// ODOMETRY SENSOR // ODOMETRY SENSOR
if (new_capture->getSensorPtr() == sensor_prior_) if (new_capture->getSensorPtr() == sensor_prior_)
{ {
//std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl; std::cout << "adding odometry capture..." << new_capture->nodeId() << std::endl;
// ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME // ADD/INTEGRATE NEW ODOMETRY TO THE LAST FRAME
last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture)); last_capture_relative_->integrateCapture((CaptureMotion*) (new_capture));
std::cout << "1..." << new_capture->nodeId() << std::endl;
current_frame_->setState(last_capture_relative_->computeFramePose(new_capture->getTimeStamp())); current_frame_->setState(last_capture_relative_->computeFramePose(new_capture->getTimeStamp()));
std::cout << "2..." << new_capture->nodeId() << std::endl;
current_frame_->setTimeStamp(new_capture->getTimeStamp()); current_frame_->setTimeStamp(new_capture->getTimeStamp());
delete new_capture; delete new_capture;
} }
else else
{ {
//std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl; std::cout << "adding not odometry capture..." << new_capture->nodeId() << std::endl;
// ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture) // ADD CAPTURE TO THE CURRENT FRAME (or substitute the same sensor previous capture)
//std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl; //std::cout << "searching repeated capture..." << new_capture->nodeId() << std::endl;
...@@ -197,16 +199,21 @@ void WolfManager::update() ...@@ -197,16 +199,21 @@ void WolfManager::update()
} }
} }
} }
//std::cout << "updated" << std::endl; std::cout << "updated" << std::endl;
} }
Eigen::VectorXs WolfManager::getVehiclePose(const TimeStamp& _now) Eigen::VectorXs WolfManager::getVehiclePose(const TimeStamp& _now)
{ {
std::cout << "getting vehicle pose... " << std::endl;
if (last_capture_relative_ == nullptr) if (last_capture_relative_ == nullptr)
return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr()); return Eigen::Map<Eigen::Vector3s>(current_frame_->getPPtr()->getPtr());
else else
{
std::cout << "last capture relative... " << std::endl;
return last_capture_relative_->computeFramePose(_now); return last_capture_relative_->computeFramePose(_now);
}
} }
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment