Skip to content
Snippets Groups Projects
frame_base.cpp 12.59 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::VectorXd& _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 PO 2D!");
        StateBlockPtr p_ptr ( std::make_shared<StateBlock>    (_x.head    <2> ( ) ) );
        StateBlockPtr o_ptr ( std::make_shared<StateAngle>    ((double) _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 PO 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::VectorXd::Zero(10);
        assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for POV 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 if(_frame_structure.compare("POVCDL") == 0 and _dim == 3){
        // auto _x = Eigen::VectorXd::Zero(10);
        assert(_x.size() == 19 && "Wrong state vector size. Should be 19 for POVCDL 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.segment <3> (7 ) ) );
        StateBlockPtr c_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (10) ) );
        StateBlockPtr cd_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (13) ) );
        StateBlockPtr Lc_ptr ( std::make_shared<StateBlock>      (_x.segment <3> (16) ) );
        addStateBlock("P", p_ptr);
        addStateBlock("O", o_ptr);
        addStateBlock("V", v_ptr);
        addStateBlock("C", c_ptr);
        addStateBlock("D", cd_ptr);
        addStateBlock("L", Lc_ptr);
        this->setType("POVCDL 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(getProblem());
    }
}

void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
    time_stamp_ = _ts;
    if (isKeyOrAux() && getTrajectory() != nullptr)
        getTrajectory()->sortFrame(shared_from_this());
}

void FrameBase::setNonEstimated()
{
    // unregister if previously estimated
    if (isKeyOrAux())
        for (const auto& sb : getStateBlockVec())
            getProblem()->notifyStateBlock(sb, REMOVE);

    type_ = NON_ESTIMATED;
    if (getTrajectory())
    {
        getTrajectory()->sortFrame(shared_from_this());
        getTrajectory()->updateLastFrames();
    }
}

void FrameBase::setKey()
{
    // register if previously not estimated
    if (!isKeyOrAux())
        registerNewStateBlocks(getProblem());

    type_ = KEY;

    if (getTrajectory())
    {
        getTrajectory()->sortFrame(shared_from_this());
        getTrajectory()->updateLastFrames();
    }
}

void FrameBase::setAux()
{
    if (!isKeyOrAux())
        registerNewStateBlocks(getProblem());

    type_ = AUXILIARY;

    if (getTrajectory())
    {
        getTrajectory()->sortFrame(shared_from_this());
        getTrajectory()->updateLastFrames();
    }
}

bool FrameBase::getCovariance(Eigen::MatrixXd& _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
{
    if (_sensor_ptr)
        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);
}

bool FrameBase::isConstrainedBy(const FactorBasePtr &_factor) const
{
    FactorBaseConstIter cby_it = std::find_if(constrained_by_list_.begin(),
                                              constrained_by_list_.end(),
                                              [_factor](const FactorBasePtr & cby)
                                              {
                                                 return cby == _factor;
                                              }
    );
    if (cby_it != constrained_by_list_.end())
        return true;
    else
        return false;
}

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())
        registerNewStateBlocks(_problem);

    for (auto cap : capture_list_)
        cap->setProblem(_problem);
}

void FrameBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
    _stream << _tabs << (isKeyOrAux() ? (isKey() ? "KFrm" : "AFrm") : "Frm") << id() << ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C  " : "");
    if (_constr_by)
    {
        _stream << "  <-- ";
        for (auto cby : getConstrainedByList())
            _stream << "Fac" << cby->id() << " \t";
    }
    _stream << std::endl;

    if (_metric && _state_blocks){
        for (const auto& key : getStructure())
        {
            auto sb = getStateBlock(key);
            _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " )" << std::endl;
        }
    }
    else if (_metric)
    {
        _stream << _tabs << "  " << (isFixed() ? "Fix" : "Est") << ", ts=" << std::setprecision(5)
                << getTimeStamp();
        _stream << ",\t x = ( " << std::setprecision(2) << getState().transpose() << " )";
        _stream << std::endl;
    }
    else if (_state_blocks)
    {
        _stream << _tabs << "  " << "sb:";
        for (const auto& key : getStructure())
        {
            const auto& sb = getStateBlock(key);
            _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
        }
        _stream << std::endl;
    }
}

void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
    printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
    if (_depth >= 2)
        for (auto C : getCaptureList())
            C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
}
} // namespace wolf