-
Joan Solà Ortega authoredJoan Solà Ortega authored
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