-
Joan Solà Ortega authoredJoan Solà Ortega authored
frame_base.cpp 9.99 KiB
#include "core/frame/frame_base.h"
#include "core/factor/factor_base.h"
#include "core/trajectory/trajectory_base.h"
#include "core/capture/capture_base.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_angle.h"
#include "core/state_block/state_quaternion.h"
namespace wolf {
unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
type_(NON_ESTIMATED),
time_stamp_(_ts)
{
if (_p_ptr)
{
addStateBlock("P", _p_ptr);
}
if (_o_ptr)
{
addStateBlock("O", _o_ptr);
}
if (_v_ptr)
{
addStateBlock("V", _v_ptr);
}
}
FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _v_ptr) :
NodeBase("FRAME", "Base"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
{
if (_p_ptr)
{
addStateBlock("P", _p_ptr);
}
if (_o_ptr)
{
addStateBlock("O", _o_ptr);
}
if (_v_ptr)
{
addStateBlock("V", _v_ptr);
}
}
FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
NodeBase("FRAME", "Base"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
type_(_tp),
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((Scalar) _x(2) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
this->setType("PO 2D");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
this->setType("PO 3D");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
// auto _x = Eigen::VectorXs::Zero(10);
assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
StateBlockPtr v_ptr ( std::make_shared<StateBlock> (_x.tail <3> ( ) ) );
addStateBlock("P", p_ptr);
addStateBlock("O", o_ptr);
addStateBlock("V", v_ptr);
this->setType("POV 3D");
}else{
std::cout << _frame_structure << " ^^ " << _dim << std::endl;
throw std::runtime_error("Unknown frame structure");
}
}
FrameBase::~FrameBase()
{
}
void FrameBase::remove(bool viral_remove_empty_parent)
{
if (!is_removing_)
{
is_removing_ = true;
FrameBasePtr this_F = shared_from_this(); // keep this alive while removing it
// remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
if (T)
{
T->removeFrame(this_F); // remove from upstream
}
// remove downstream
while (!capture_list_.empty())
{
capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
while (!constrained_by_list_.empty())
{
constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained
}
// Remove Frame State Blocks
if ( isKeyOrAux() )
removeStateBlocks();
}
}
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
if (isKeyOrAux() && getTrajectory() != nullptr)
getTrajectory()->sortFrame(shared_from_this());
}
void FrameBase::registerNewStateBlocks() const
{
if (getProblem() != nullptr)
{
for (auto pair_key_sbp : getStateBlockMap())
if (pair_key_sbp.second != nullptr)
getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
}
}
void FrameBase::removeStateBlocks()
{
for (const char key : getStructure()) // note: key is a char
{
auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->notifyStateBlock(sbp,REMOVE);
}
}
removeStateBlock(key);
}
}
void FrameBase::setNonEstimated()
{
// unregister if previously estimated
if (isKeyOrAux())
removeStateBlocks();
type_ = NON_ESTIMATED;
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
void FrameBase::setKey()
{
// register if previously not estimated
if (!isKeyOrAux())
registerNewStateBlocks();
// WOLF_DEBUG("Set Key", this->id());
type_ = KEY;
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
void FrameBase::setAux()
{
if (!isKeyOrAux())
registerNewStateBlocks();
// WOLF_DEBUG("Set Auxiliary", this->id());
type_ = AUXILIARY;
if (getTrajectory())
{
getTrajectory()->sortFrame(shared_from_this());
getTrajectory()->updateLastFrames();
}
}
void FrameBase::setState(const Eigen::VectorXs& _state)
{
HasStateBlocks::setState(_state, isKeyOrAux());
}
bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
{
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
FrameBasePtr FrameBase::getPreviousFrame() const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
{
if ( this->frame_id_ == (*f_it)->id() )
{
f_it++;
if (f_it != getTrajectory()->getFrameList().rend())
{
return *f_it;
}
else
{
return nullptr;
}
}
}
return nullptr;
}
FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
auto f_it = getTrajectory()->getFrameList().rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory
while (f_it != getTrajectory()->getFrameList().rend())
{
if ( this->frame_id_ == (*f_it)->id())
{
f_it--;
return *f_it;
}
f_it++;
}
std::cout << "next frame not found!" << std::endl;
return nullptr;
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
capture_list_.push_back(_capt_ptr);
return _capt_ptr;
}
void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
{
capture_list_.remove(_capt_ptr);
}
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
CaptureBasePtr FrameBase::getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
return nullptr;
}
CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr) const
{
CaptureBasePtrList captures;
for (CaptureBasePtr capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
return captures;
}
FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type) const
{
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
return nullptr;
}
FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) const
{
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
return nullptr;
}
void FrameBase::getFactorList(FactorBasePtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
c_ptr->getFactorList(_fac_list);
}
FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.push_back(_fac_ptr);
return _fac_ptr;
}
void FrameBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
constrained_by_list_.remove(_fac_ptr);
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
{
assert(!is_removing_ && "linking a removed frame");
assert(this->getTrajectory() == nullptr && "linking an already linked frame");
if(_trj_ptr)
{
_trj_ptr->addFrame(shared_from_this());
this->setTrajectory(_trj_ptr);
this->setProblem(_trj_ptr->getProblem());
}
else
{
WOLF_WARN("Linking with a nullptr");
}
}
void FrameBase::setProblem(ProblemPtr _problem)
{
if (_problem == nullptr || _problem == this->getProblem())
return;
NodeBase::setProblem(_problem);
if (this->isKey())
this->registerNewStateBlocks();
for (auto cap : capture_list_)
cap->setProblem(_problem);
}
} // namespace wolf