Skip to content
Snippets Groups Projects
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