capture_base.cpp

#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->getP() != nullptr and _sensor_ptr->isStateBlockDynamic("P"))
        {
            assert(_p_ptr && "Pointer to dynamic position params is null!");
            addStateBlock("P", _p_ptr, nullptr);
        }
        else
            assert(_p_ptr == nullptr && "Provided dynamic sensor position but the sensor position is static or doesn't exist");

        if (_sensor_ptr->getO() != nullptr and _sensor_ptr->isStateBlockDynamic("O"))
        {
            assert(_o_ptr && "Pointer to dynamic orientation params is null!");
            addStateBlock("O", _o_ptr, nullptr);
        }
        else
            assert(_o_ptr == nullptr && "Provided dynamic sensor orientation but the sensor orientation is static or doesn't exist");

        if (_sensor_ptr->getIntrinsic() != nullptr and _sensor_ptr->isStateBlockDynamic("I"))
        {
            assert(_intr_ptr && "Pointer to dynamic intrinsic params is null!");
            addStateBlock("I", _intr_ptr, nullptr);
        }
        else
            assert(_intr_ptr == nullptr && "Provided dynamic sensor intrinsics but the sensor intrinsics are static or don't exist");

    }
    else if (_p_ptr || _o_ptr || _intr_ptr)
    {
        WOLF_ERROR("Provided sensor parameters but no sensor pointer");
    }

    updateCalibSize();
}

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

void CaptureBase::remove(bool viral_remove_empty_parent)
{
    /* Order of removing:
     * Factors are removed first, and afterwards the rest of nodes containing state blocks.
     * In case multi-threading, solver can be called while removing.
     * This order avoids SolverManager to ignore notifications (efficiency)
     */
    if (!is_removing_)
    {
        WOLF_INFO("CaptureBase::remove");
        is_removing_ = true;
        CaptureBasePtr this_C = shared_from_this();  // keep this alive while removing it

        // remove downstream
        while (!constrained_by_list_.empty())
        {
            constrained_by_list_.front()->remove(viral_remove_empty_parent); // remove constrained by
        }
        while (!feature_list_.empty())
        {
            feature_list_.front()->remove(viral_remove_empty_parent); // remove downstream
        }

        // Remove State Blocks
        removeStateBlocks(getProblem());

        // 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
        }
    }
}

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);
}

bool CaptureBase::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;
}


const std::string& CaptureBase::getStructure() const
{
    if (getSensor())
        return getSensor()->getStructure();
    else
        return HasStateBlocks::getStructure();
}

StateBlockPtr CaptureBase::getStateBlock(const std::string& _key) const
{
    if (getSensor() and getSensor()->hasStateBlock(_key))
    {
        if (getSensor()->isStateBlockDynamic(_key))
            return HasStateBlocks::getStateBlock(_key);
        else
            return getSensor()->getStateBlock(_key);
    }
    else // No sensor associated, or sensor without this state block: assume sensor params are here
        return HasStateBlocks::getStateBlock(_key);
}

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

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

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

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

void CaptureBase::setCalibration(const VectorXd& _calib)
{
    updateCalibSize();
    assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
    SizeEigen index = 0;
    for (const auto& key : getStructure())
    {
        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");

    assert((this->getFrame() == nullptr || this->getFrame()->isKey()) && "Forbidden: moving a capture already linked to a KF");
    assert((_frm_ptr == nullptr || _frm_ptr->isKey()) && "Forbidden: moving a capture to a non-estimated frame");

    // Unlink
    if (this->getFrame())
    {
        // 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);
    registerNewStateBlocks(_problem);

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

void CaptureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
    _stream << _tabs << "Cap" << id() << " " << getType();

    if(getSensor() != nullptr)
    {
        _stream << " -> Sen" << getSensor()->id();
    }
    else
        _stream << " -> Sen-";

    _stream << ((_depth < 3) ? " -- " + std::to_string(getFeatureList().size()) + "f" : "");
    if (_constr_by)
    {
        _stream << "  <-- ";
        for (auto cby : getConstrainedByList())
            _stream << "Fac" << cby->id() << " \t";
    }
    _stream << std::endl;

    if(getStateBlockMap().size() > 0)
    {
        if (_metric && _state_blocks){
            for (const auto& key : getStructure())
            {
                auto sb = getStateBlock(key);
                _stream << _tabs << 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 << "    sb:";
            for (const auto& key : getStructure())
            {
                const auto& sb = getStateBlock(key);
                _stream << "    " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "]; ";
            }
            _stream << std::endl;
        }
    }



//    if (_state_blocks)
//        for (const auto& sb : getStateBlockVec())
//        {
//            if(sb != nullptr)
//            {
//                _stream << "      sb: ";
//                _stream << (sb->isFixed() ? "Fix" : "Est");
//                if (_metric)
//                    _stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
//                _stream << std::endl;
//            }
//        }
}
void CaptureBase::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 >= 3)
        for (auto f : getFeatureList())
            f->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + "  ");
}
} // namespace wolf