Skip to content
Snippets Groups Projects
capture_base.cpp 8.09 KiB
#include "core/capture/capture_base.h"
#include "core/sensor/sensor_base.h"

namespace wolf{

using namespace Eigen;

unsigned int CaptureBase::capture_id_count_ = 0;

CaptureBase::CaptureBase(const std::string& _type,
                         const TimeStamp& _ts,
                         SensorBasePtr _sensor_ptr,
                         StateBlockPtr _p_ptr,
                         StateBlockPtr _o_ptr,
                         StateBlockPtr _intr_ptr) :
    NodeBase("CAPTURE", _type),
    HasStateBlocks(""),
    frame_ptr_(), // nullptr
    sensor_ptr_(_sensor_ptr),
    calib_size_(0),
    capture_id_(++capture_id_count_),
    time_stamp_(_ts)
{
    if (_sensor_ptr)
    {

        if (_sensor_ptr->isExtrinsicDynamic())
        {
            assert(_p_ptr && "Pointer to dynamic position params is null!");
            setStateBlock("P", _p_ptr);
            appendToStructure("P");

            assert(_o_ptr && "Pointer to dynamic orientation params is null!");
            setStateBlock("O", _o_ptr);
            appendToStructure("O");
        }
        else if (_p_ptr || _o_ptr)
        {
            WOLF_ERROR("Provided dynamic sensor extrinsics but the sensor extrinsics are static");
        }

        if (_sensor_ptr->isIntrinsicDynamic())
        {
            assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
            setStateBlock("I", _intr_ptr);
            appendToStructure("I");
        }
        else if (_intr_ptr)
        {
            WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
        }

        getSensor()->setHasCapture();
    }
    else if (_p_ptr || _o_ptr || _intr_ptr)
    {
        WOLF_ERROR("Provided sensor parameters but no sensor pointer");
    }
    updateCalibSize();

//    WOLF_TRACE("New Capture ", id(), " -- type ", getType(), " -- t = ", getTimeStamp(), " s");
}

CaptureBase::~CaptureBase()
{
    removeStateBlocks();
}

void CaptureBase::remove(bool viral_remove_empty_parent)
{
    if (!is_removing_)
    {
        is_removing_ = true;
        CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it

        // Remove State Blocks
        removeStateBlocks();

        // remove from upstream
        FrameBasePtr F = frame_ptr_.lock();
        if (F)
        {
            F->removeCapture(this_C);
            if (viral_remove_empty_parent && F->getCaptureList().empty() && F->getConstrainedByList().empty())
                F->remove(viral_remove_empty_parent); // remove upstream
        }

        // remove downstream
        while (!feature_list_.empty())
        {
            feature_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 by
        }
    }
}

bool CaptureBase::process()
{
    assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");

    return getSensor()->process(shared_from_this());
}



FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
    //std::cout << "Adding feature" << std::endl;
    feature_list_.push_back(_ft_ptr);
    return _ft_ptr;
}

void CaptureBase::removeFeature(FeatureBasePtr _ft_ptr)
{
    feature_list_.remove(_ft_ptr);
}

void CaptureBase::getFactorList(FactorBasePtrList& _fac_list) const
{
    for (auto f_ptr : getFeatureList())
        f_ptr->getFactorList(_fac_list);
}

FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
    constrained_by_list_.push_back(_fac_ptr);
    return _fac_ptr;
}

void CaptureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
{
    constrained_by_list_.remove(_fac_ptr);
}

StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
{
    if (getSensor())
    {
        if (_key == "P") // extrinsics
        {
            if (getSensor()->extrinsicsInCaptures())
                return HasStateBlocks::getStateBlock(_key);
            else
                return getSensor()->getStateBlockPtrStatic("P");
        }

        if (_key == "O") // extrinsics
        {
            if (getSensor()->extrinsicsInCaptures())
                return HasStateBlocks::getStateBlock(_key);
            else
                return getSensor()->getStateBlockPtrStatic("O");
        }

        else // intrinsics
        {
            if (getSensor()->intrinsicsInCaptures())
                return HasStateBlocks::getStateBlock(_key);
            else
                return getSensor()->getStateBlockPtrStatic("I");
        }
    }
    else // No sensor associated: assume sensor params are here
        return HasStateBlocks::getStateBlock(_key);
}

void CaptureBase::removeStateBlocks()
{
    for (const auto& key : getStructure()) // note: key is a char
    {
        auto sbp = HasStateBlocks::getStateBlock(key);
        if (sbp != nullptr)
        {
            if (getProblem() != nullptr)
            {
                getProblem()->notifyStateBlock(sbp, REMOVE);
            }
        }
        removeStateBlock(key);
    }
}

void CaptureBase::fix()
{
    HasStateBlocks::fix();
    updateCalibSize();
}

void CaptureBase::unfix()
{
    HasStateBlocks::unfix();
    updateCalibSize();
}

void CaptureBase::registerNewStateBlocks()
{
    if (getProblem() != nullptr)
    {
        for (auto pair_key_sbp : getStateBlockMap())
            if (pair_key_sbp.second != nullptr)
                getProblem()->notifyStateBlock(pair_key_sbp.second,ADD);
    }
}

SizeEigen CaptureBase::computeCalibSize() const
{
    SizeEigen sz = 0;
    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
    for (const auto& key : structure)
    {
        auto sb = getStateBlock(key);
        if (sb && !sb->isFixed())
            sz += sb->getSize();
    }
    return sz;
}

Eigen::VectorXs CaptureBase::getCalibration() const
{
    Eigen::VectorXs calib(calib_size_);
    SizeEigen index = 0;
    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
    for (const auto& key : structure)
    {
        auto sb = getStateBlock(key);
        if (sb && !sb->isFixed())
        {
            calib.segment(index, sb->getSize()) = sb->getState();
            index += sb->getSize();
        }
    }
    return calib;
}

void CaptureBase::setCalibration(const VectorXs& _calib)
{
    updateCalibSize();
    assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
    SizeEigen index = 0;
    const auto& structure = (getSensor() ? getSensor()->getStructure() : getStructure());
    for (const auto& key : structure)
    {
        auto sb = getStateBlock(key);
        if (sb && !sb->isFixed())
        {
            sb->setState(_calib.segment(index, sb->getSize()));
            index += sb->getSize();
        }
    }
}

void CaptureBase::move(FrameBasePtr _frm_ptr)
{
    WOLF_WARN_COND(this->getFrame() == nullptr, "moving a capture not linked to any frame");
    WOLF_WARN_COND(_frm_ptr == nullptr, "moving a capture to a null FrameBasePtr");

    // Unlink
    if (this->getFrame())
    {
        if (this->getFrame()->isKey())
        {
            WOLF_ERROR("moving a capture linked to a KF");
            return;
        }

        // unlink from previous non-key frame
        this->getFrame()->removeCapture(shared_from_this());
        this->setFrame(nullptr);
    }

    // link
    link(_frm_ptr);
}


void CaptureBase::link(FrameBasePtr _frm_ptr)
{
    assert(!is_removing_ && "linking a removed capture");
    assert(this->getFrame() == nullptr && "linking a capture already linked");

    if(_frm_ptr)
    {

        _frm_ptr->addCapture(shared_from_this());
        this->setFrame(_frm_ptr);
        this->setProblem(_frm_ptr->getProblem());
    }
    else
    {
        WOLF_WARN("Linking with a nullptr");
    }
}

void CaptureBase::setProblem(ProblemPtr _problem)
{
    if (_problem == nullptr || _problem == this->getProblem())
        return;

    NodeBase::setProblem(_problem);
    this->registerNewStateBlocks();

    for (auto ft : feature_list_)
        ft->setProblem(_problem);
}

} // namespace wolf