-
Joaquim Casals Buñuel authoredJoaquim Casals Buñuel authored
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