-
Joaquim Casals Buñuel authoredJoaquim Casals Buñuel authored
problem.cpp 76.74 KiB
// wolf includes
#include "core/problem/problem.h"
#include "core/hardware/hardware_base.h"
#include "core/trajectory/trajectory_base.h"
#include "core/map/map_base.h"
#include "core/sensor/sensor_base.h"
#include "core/processor/processor_motion.h"
#include "core/processor/processor_tracker.h"
#include "core/capture/capture_pose.h"
#include "core/factor/factor_base.h"
#include "core/sensor/sensor_factory.h"
#include "core/processor/processor_factory.h"
#include "core/state_block/state_block.h"
#include "core/utils/logging.h"
#include "core/utils/params_server.hpp"
#include "core/utils/loader.hpp"
#include "core/utils/check_log.hpp"
// IRI libs includes
// C++ includes
#include <algorithm>
#include <map>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <unordered_set>
namespace wolf
{
Problem::Problem(const std::string& _frame_structure, SizeEigen _dim) :
hardware_ptr_(std::make_shared<HardwareBase>()),
trajectory_ptr_(std::make_shared<TrajectoryBase>(_frame_structure)),
map_ptr_(std::make_shared<MapBase>()),
processor_motion_ptr_(),
prior_is_set_(false),
frame_structure_(_frame_structure)
{
if (_frame_structure == "PO" and _dim == 2)
{
state_size_ = 3;
state_cov_size_ = 3;
dim_ = 2;
}else if (_frame_structure == "PO" and _dim == 3)
{
state_size_ = 7;
state_cov_size_ = 6;
dim_ = 3;
} else if (_frame_structure == "POV" and _dim == 3)
{
state_size_ = 10;
state_cov_size_ = 9;
dim_ = 3;
}
else std::runtime_error(
"Problem::Problem(): Unknown frame structure. Add appropriate frame structure to the switch statement.");
}
void Problem::setup()
{
hardware_ptr_ -> setProblem(shared_from_this());
trajectory_ptr_-> setProblem(shared_from_this());
map_ptr_ -> setProblem(shared_from_this());
}
ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim)
{
ProblemPtr p(new Problem(_frame_structure, _dim)); // We use `new` and not `make_shared` since the Problem constructor is private and cannot be passed to `make_shared`.
p->setup();
return p->shared_from_this();
}
ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
#if __APPLE__
std::string lib_extension = ".dylib";
#else
std::string lib_extension = ".so";
#endif
// Problem structure and dimension
std::string frame_structure = _server.getParam<std::string> ("problem/frame_structure");
int dim = _server.getParam<int> ("problem/dimension");
auto problem = Problem::create(frame_structure, dim);
//
// cout << "PRINTING SERVER MAP" << endl;
// _server.print();
// cout << "-----------------------------------" << endl;
WOLF_TRACE("Setting up problem with frame structure {" + frame_structure + "} and dimension " + std::to_string(dim) + "D");
// Load plugins
auto loaders = std::vector<Loader *>();
std::string plugins_path;
try{
plugins_path = _server.getParam<std::string>("plugins_path");
}
catch(MissingValueException& e){
WOLF_WARN(e.what());
WOLF_WARN("Setting '/usr/local/lib/iri-algorithms/' as plugins path...");
plugins_path="/usr/local/lib/iri-algorithms/";
}
for (auto plugin_name : _server.getParam<std::vector<std::string>>("plugins")) {
if (plugin_name == "core" or plugin_name == "wolf" or plugin_name == "") continue; // ignore plugin "core"
std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension;
WOLF_TRACE("Loading plugin " + plugin);
auto l = new LoaderRaw(plugin);
l->load();
loaders.push_back(l);
}
std::string packages_path;
try {
packages_path = _server.getParam<std::string>("packages_path");
} catch (MissingValueException& e) {
WOLF_WARN(e.what());
WOLF_WARN("Support for subscribers disabled...");
}
for (auto it : _server.getParam<std::vector<std::string>>("packages")) {
std::string subscriber = packages_path + "/libsubscriber_" + it + lib_extension;
WOLF_TRACE("Loading subscriber " + subscriber);
auto l = new LoaderRaw(subscriber);
l->load();
loaders.push_back(l);
}
std::vector<std::string> raw_libs;
try {
raw_libs = _server.getParam<std::vector<std::string>>("raw_libs");
} catch (MissingValueException& e) {
WOLF_TRACE("No raw libraries to load...");
raw_libs = std::vector<std::string>();
}
for (auto lib : raw_libs) {
WOLF_TRACE("Loading raw lib " + lib);
auto l = new LoaderRaw(lib);
l->load();
loaders.push_back(l);
}
// Install sensors and processors
auto sensorMap = std::map<std::string, SensorBasePtr>();
auto procesorMap = std::map<std::string, ProcessorBasePtr>();
auto sensors = _server.getParam<std::vector<std::map<std::string, std::string>>>("sensors");
for(auto sen : sensors){
sensorMap.insert(std::pair<std::string, SensorBasePtr>(sen["name"], problem->installSensor(sen["type"], sen["name"], _server)));
}
auto processors = _server.getParam<std::vector<std::map<std::string, std::string>>>("processors");
for(auto prc : processors){
procesorMap.insert(std::pair<std::string, ProcessorBasePtr>(prc["name"], problem->installProcessor(prc["type"], prc["name"], prc["sensor_name"], _server)));
}
// Prior
Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
double prior_time_tolerance = _server.getParam<double>("problem/prior/time_tolerance");
double prior_ts = _server.getParam<double>("problem/prior/timestamp");
WOLF_TRACE("prior timestamp:\n" , prior_ts);
WOLF_TRACE("prior state:\n" , prior_state.transpose());
WOLF_TRACE("prior covariance:\n" , prior_cov);
WOLF_TRACE("prior time tolerance:\n", prior_time_tolerance);
problem->setPrior(prior_state, prior_cov, prior_ts, prior_time_tolerance);
// Done
return problem;
}
Problem::~Problem()
{
// WOLF_DEBUG("destructed -P");
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXd& _extrinsics, //
ParamsSensorBasePtr _intrinsics)
{
SensorBasePtr sen_ptr = SensorFactory::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics);
sen_ptr->link(getHardware());
return sen_ptr;
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
if (_intrinsics_filename != "")
{
assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist.");
ParamsSensorBasePtr intr_ptr = ParamsSensorFactory::get().create(_sen_type, _intrinsics_filename);
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr);
}
else
return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr());
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
const ParamsServer& _server)
{
SensorBasePtr sen_ptr = AutoConfSensorFactory::get().create(_sen_type, _unique_sensor_name, _server);
sen_ptr->link(getHardware());
return sen_ptr;
}
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
SensorBasePtr _corresponding_sensor_ptr, //
ProcessorParamsBasePtr _prc_params)
{
if (_corresponding_sensor_ptr == nullptr)
{
WOLF_ERROR("Cannot install processor '", _unique_processor_name,
"' since the associated sensor does not exist !");
return ProcessorBasePtr();
}
ProcessorBasePtr prc_ptr = ProcessorFactory::get().create(_prc_type, _unique_processor_name, _prc_params);
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(_corresponding_sensor_ptr);
prc_ptr->link(_corresponding_sensor_ptr);
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(prc_ptr);
return prc_ptr;
}
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename)
{
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
if (_params_filename == "")
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, nullptr);
else
{
assert(file_exists(_params_filename) && "Cannot install processor: parameters' YAML file does not exist.");
ProcessorParamsBasePtr prc_params = ProcessorParamsFactory::get().create(_prc_type, _params_filename);
return installProcessor(_prc_type, _unique_processor_name, sen_ptr, prc_params);
}
}
ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _unique_processor_name, //
const std::string& _corresponding_sensor_name, //
const ParamsServer& _server)
{
SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Cannot bind processor. Reason: Sensor \"" + _corresponding_sensor_name + "\" not found. Check sensor name, it must match in sensor and processor!");
ProcessorBasePtr prc_ptr = AutoConfProcessorFactory::get().create(_prc_type, _unique_processor_name, _server);
//Dimension check
int prc_dim = prc_ptr->getDim();
auto prb = this;
assert(((prc_dim == 0) or (prc_dim == prb->getDim())) && "Processor and Problem do not agree on dimension");
prc_ptr->configure(sen_ptr);
prc_ptr->link(sen_ptr);
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
(std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
processor_motion_ptr_ = std::static_pointer_cast<ProcessorMotion>(prc_ptr);
return prc_ptr;
}
SensorBasePtr Problem::getSensor(const std::string& _sensor_name) const
{
auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
getHardware()->getSensorList().end(),
[&](SensorBasePtr sb)
{
return sb->getName() == _sensor_name;
}); // lambda function for the find_if
if (sen_it == getHardware()->getSensorList().end())
return nullptr;
return (*sen_it);
}
IsMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
sen->getProcessorList().end(),
[&](ProcessorBasePtr prc)
{
return prc->getName() == _processor_name;
}); // lambda function for the find_if
if (prc_it != sen->getProcessorList().end()) // found something!
{
if ((*prc_it)->isMotion()) // found, and it's motion!
{
std::cout << "Found processor '" << _processor_name << "', of type Motion, and set as the main motion processor." << std::endl;
processor_motion_ptr_ = std::dynamic_pointer_cast<IsMotion>(*prc_it);
return processor_motion_ptr_;
}
else // found, but it's not motion!
{
std::cout << "Found processor '" << _processor_name << "', but not of type Motion!" << std::endl;
return nullptr;
}
}
}
// nothing found!
std::cout << "Processor '" << _processor_name << "' not found!" << std::endl;
return nullptr;
}
void Problem::setProcessorMotion(IsMotionPtr _processor_motion_ptr)
{
processor_motion_ptr_ = _processor_motion_ptr;
}
void Problem::clearProcessorMotion()
{
processor_motion_ptr_.reset();
}
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp)
{
auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
return frm;
}
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
const TimeStamp& _time_stamp)
{
return emplaceFrame(_frame_structure, _dim, _frame_key_type, getState(_time_stamp), _time_stamp);
}
FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp)
{
return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
}
FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
const TimeStamp& _time_stamp)
{
return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
}
Eigen::VectorXd Problem::getCurrentState() const
{
Eigen::VectorXd state(getFrameStructureSize());
getCurrentState(state);
return state;
}
void Problem::getCurrentState(Eigen::VectorXd& _state) const
{
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(_state);
else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
else
_state = zeroState();
}
void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
{
if (processor_motion_ptr_ != nullptr)
{
processor_motion_ptr_->getCurrentState(_state);
processor_motion_ptr_->getCurrentTimeStamp(ts);
}
else if (trajectory_ptr_->getLastKeyOrAuxFrame() != nullptr)
{
trajectory_ptr_->getLastKeyOrAuxFrame()->getTimeStamp(ts);
trajectory_ptr_->getLastKeyOrAuxFrame()->getState(_state);
}
else
_state = zeroState();
}
void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
{
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
{
FrameBasePtr closest_frame = trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
if (closest_frame != nullptr)
closest_frame->getState(_state);
else
_state = zeroState();
}
}
Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
{
Eigen::VectorXd state(getFrameStructureSize());
getState(_ts, state);
return state;
}
SizeEigen Problem::getFrameStructureSize() const
{
return state_size_;
}
void Problem::getFrameStructureSize(SizeEigen& _x_size, SizeEigen& _cov_size) const
{
_x_size = state_size_;
_cov_size = state_cov_size_;
}
SizeEigen Problem::getDim() const
{
return dim_;
}
std::string Problem::getFrameStructure() const
{
return frame_structure_;
}
Eigen::VectorXd Problem::zeroState() const
{
Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
// Set the quaternion identity for 3d states. Set only the real part to 1:
if(this->getDim() == 3)
state(6) = 1.0;
return state;
}
bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) const
{
// This should implement a black list of processors that have forbidden keyframe creation
// This decision is made at problem level, not at processor configuration level.
// If you want to configure a processor for not creating keyframes, see Processor::voting_active_ and its accessors.
// Currently allowing all processors to vote:
return true;
}
void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
WOLF_DEBUG_COND(_processor_ptr!=nullptr,(_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
WOLF_DEBUG_COND(_processor_ptr==nullptr,"External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp());
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor && (processor != _processor_ptr) )
{
#ifdef PROFILING
auto start = std::chrono::high_resolution_clock::now();
#endif
processor->keyFrameCallback(_keyframe_ptr, _time_tolerance);
#ifdef PROFILING
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
WOLF_INFO("keyFrameCallback Prc_id: ", processor->id(),
" Prc_type: ", processor->getType(),
" Prc_name: ", processor->getName(),
" keyframe_id: ", _keyframe_ptr->id(),
" keyframe_type: ", _keyframe_ptr->getType(),
" timestamp: ", _keyframe_ptr->getTimeStamp(),
" microseconds: ", duration.count());
#endif
}
}
bool Problem::permitAuxFrame(ProcessorBasePtr _processor_ptr) const
{
// This should implement a black list of processors that have forbidden auxiliary frame creation
// This decision is made at problem level, not at processor configuration level.
// If you want to configure a processor for not creating auxiliary frames, see Processor::voting_active_ and its accessors.
// Currently allowing all processors to vote:
return true;
}
void Problem::auxFrameCallback(FrameBasePtr _frame_ptr, ProcessorBasePtr _processor_ptr, const double& _time_tolerance)
{
// TODO
// if (_processor_ptr)
// {
// WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
// }
// else
// {
// WOLF_DEBUG("External callback: AuxF", _frame_ptr->id(), " Callback emitted with ts = ", _frame_ptr->getTimeStamp());
// }
//
// processor_motion_ptr_->keyFrameCallback(_frame_ptr, _time_tolerance);
}
StateBlockPtr Problem::notifyStateBlock(StateBlockPtr _state_ptr, Notification _noti)
{
std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
//std::cout << "Problem::addStateBlockPtr " << _state_ptr.get() << std::endl;
// Check if there is already a notification for this state block
auto notification_it = state_block_notification_map_.find(_state_ptr);
// exsiting notification for this state block
if (notification_it != state_block_notification_map_.end())
{
// duplicated notification
if ( notification_it->second == _noti)
{
WOLF_WARN("This notification has been already notified");
}
// opposite notification -> cancell out eachother
else
state_block_notification_map_.erase(notification_it);
}
// Add notification
else
state_block_notification_map_[_state_ptr] = _noti;
return _state_ptr;
}
bool Problem::getStateBlockNotification(const StateBlockPtr& sb_ptr, Notification& notif) const
{
std::lock_guard<std::mutex> lock(mut_state_block_notifications_);
if (state_block_notification_map_.find(sb_ptr) == state_block_notification_map_.end())
return false;
notif = state_block_notification_map_.at(sb_ptr);
return true;
}
FactorBasePtr Problem::notifyFactor(FactorBasePtr _factor_ptr, Notification _noti)
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
//std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already the same notification for this factor
auto notification_it = factor_notification_map_.find(_factor_ptr);
// exsiting notification for this factor
if (notification_it != factor_notification_map_.end())
{
// duplicated notification
if (notification_it->second == _noti)
{
WOLF_WARN("This notification has been already notified");
}
// opposite notification -> cancell out eachother
else
factor_notification_map_.erase(notification_it);
}
// Add notification
else
factor_notification_map_[_factor_ptr] = _noti;
return _factor_ptr;
}
bool Problem::getFactorNotification(const FactorBasePtr& fac_ptr, Notification& notif) const
{
std::lock_guard<std::mutex> lock(mut_factor_notifications_);
if (factor_notification_map_.find(fac_ptr) == factor_notification_map_.end())
return false;
notif = factor_notification_map_.at(fac_ptr);
return true;
}
void Problem::clearCovariance()
{
std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_.clear();
}
void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
std::lock_guard<std::mutex> lock(mut_covariances_);
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
//std::cout << "_row " << _row << std::endl;
//std::cout << "_col " << _col << std::endl;
//std::cout << "_state1 tangent space size: " << _state1->getLocalSize() << std::endl;
//std::cout << "_state2 tangent space size: " << _state2->getLocalSize() << std::endl;
//std::cout << "part of cov to be filled:" << std::endl << _cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) << std::endl;
//if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
// std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] << std::endl;
//else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
// std::cout << "stored cov" << std::endl << covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)].transpose() << std::endl;
assert(_row + _state1->getLocalSize() <= _cov.rows() && _col + _state2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
std::lock_guard<std::mutex> lock(mut_covariances_);
if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2));
else if (covariances_.find(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)) != covariances_.end())
_cov.block(_row, _col, _state1->getLocalSize(), _state2->getLocalSize()) =
covariances_.at(std::pair<StateBlockPtr, StateBlockPtr>(_state2, _state1)).transpose();
else
{
WOLF_DEBUG("Could not find the requested covariance block.");
return false;
}
return true;
}
bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
// fill covariance
for (auto it1 = _sb_2_idx.begin(); it1 != _sb_2_idx.end(); it1++)
for (auto it2 = it1; it2 != _sb_2_idx.end(); it2++)
{
StateBlockPtr sb1 = it1->first;
StateBlockPtr sb2 = it2->first;
std::pair<StateBlockPtr, StateBlockPtr> pair_12(sb1, sb2);
std::pair<StateBlockPtr, StateBlockPtr> pair_21(sb2, sb1);
// search st1 & st2
if (covariances_.find(pair_12) != covariances_.end())
{
assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
_cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_12);
_cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_12).transpose();
}
else if (covariances_.find(pair_21) != covariances_.end())
{
assert(_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
assert(_sb_2_idx[sb2] + sb2->getLocalSize() <= _cov.rows() &&
_sb_2_idx[sb1] + sb1->getLocalSize() <= _cov.cols() && "Problem::getCovarianceBlock: Bad matrix covariance size!");
_cov.block(_sb_2_idx[sb1], _sb_2_idx[sb2], sb1->getLocalSize(), sb2->getLocalSize()) = covariances_.at(pair_21).transpose();
_cov.block(_sb_2_idx[sb2], _sb_2_idx[sb1], sb2->getLocalSize(), sb1->getLocalSize()) = covariances_.at(pair_21);
}
else
return false;
}
return true;
}
bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const
{
bool success(true);
// resizing
SizeEigen sz = _frame_ptr->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
for (const auto& sb_i : _frame_ptr->getStateBlockVec())
{
j = 0;
for (const auto& sb_j : _frame_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
i += sb_i->getLocalSize();
}
return success;
}
bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
{
FrameBasePtr frm = getLastKeyFrame();
return getFrameCovariance(frm, cov);
}
bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
{
FrameBasePtr frm = getLastKeyOrAuxFrame();
return getFrameCovariance(frm, cov);
}
bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
{
bool success(true);
// resizing
SizeEigen sz = _landmark_ptr->getLocalSize();
_covariance.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
for (const auto& sb_i : _landmark_ptr->getStateBlockVec())
{
j = 0;
for (const auto& sb_j : _landmark_ptr->getStateBlockVec())
{
success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
j += sb_j->getLocalSize();
}
i += sb_i->getLocalSize();
}
return success;
// bool success(true);
// int i = 0, j = 0;
//
// const auto& state_bloc_vec = _landmark_ptr->getStateBlockVec();
//
// // computing size
// SizeEigen sz = 0;
// for (const auto& sb : state_bloc_vec)
// if (sb)
// sz += sb->getLocalSize();
//
// // resizing
// _covariance = Eigen::MatrixXs(sz, sz);
//
// // filling covariance
//
// for (const auto& sb_i : state_bloc_vec)
// {
// if (sb_i)
// {
// j = 0;
// for (const auto& sb_j : state_bloc_vec)
// {
// if (sb_j)
// {
// success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j);
// j += sb_j->getLocalSize();
// }
// }
// i += sb_i->getLocalSize();
// }
// }
// return success;
}
MapBasePtr Problem::getMap() const
{
return map_ptr_;
}
TrajectoryBasePtr Problem::getTrajectory() const
{
return trajectory_ptr_;
}
HardwareBasePtr Problem::getHardware() const
{
return hardware_ptr_;
}
FrameBasePtr Problem::getLastFrame() const
{
return trajectory_ptr_->getLastFrame();
}
FrameBasePtr Problem::getLastKeyFrame() const
{
return trajectory_ptr_->getLastKeyFrame();
}
FrameBasePtr Problem::getLastKeyOrAuxFrame() const
{
return trajectory_ptr_->getLastKeyOrAuxFrame();
}
FrameBasePtr Problem::closestKeyFrameToTimeStamp(const TimeStamp& _ts) const
{
return trajectory_ptr_->closestKeyFrameToTimeStamp(_ts);
}
FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) const
{
return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}
FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen::MatrixXd& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
{
if ( ! prior_is_set_ )
{
// Create origin frame
FrameBasePtr origin_keyframe = emplaceFrame(KEY, _prior_state, _ts);
// create origin capture with the given state as data
// Capture fix only takes 3d position and Quaternion orientation
CapturePosePtr init_capture;
if (this->getFrameStructure() == "POV" and this->getDim() == 3)
init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6));
else
init_capture = CaptureBase::emplace<CapturePose>(origin_keyframe, _ts, nullptr, _prior_state, _prior_cov);
// emplace feature and factor
init_capture->emplaceFeatureAndFactor();
WOLF_DEBUG("Set prior callback: KF", origin_keyframe->id(), " Callback emitted with ts = ", origin_keyframe->getTimeStamp());
// Notify all motion processors about the prior KF
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if (processor->isMotion())
// Motion processors will set its origin at the KF
(std::static_pointer_cast<ProcessorMotion>(processor))->setOrigin(origin_keyframe);
prior_is_set_ = true;
// Notify all other processors about the origin KF --> they will join it or not depending on their received data
for (auto sensor : hardware_ptr_->getSensorList())
for (auto processor : sensor->getProcessorList())
if ( !processor->isMotion() )
processor->keyFrameCallback(origin_keyframe, _time_tolerance);
return origin_keyframe;
}
else
throw std::runtime_error("Origin already set!");
}
void Problem::loadMap(const std::string& _filename_dot_yaml)
{
getMap()->load(_filename_dot_yaml);
}
void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
{
getMap()->save(_filename_dot_yaml, _map_name);
}
void Problem::print(int depth, std::ostream& stream, bool constr_by, bool metric, bool state_blocks) const
{
stream << std::endl;
stream << "P: wolf tree status ---------------------" << std::endl;
stream << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << std::endl;
if (depth >= 1)
{
// Sensors =======================================================================================
for (auto S : getHardware()->getSensorList())
{
stream << " Sen" << S->id() << " " << S->getType() << " \"" << S->getName() << "\"";
if (depth < 2)
stream << " -- " << S->getProcessorList().size() << "p";
stream << std::endl;
if (metric && state_blocks)
{
stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "] = ( " << sb->getState().transpose() << " ); ";
}
stream << std::endl;
}
else if (metric)
{
stream << " ( ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
stream << sb->getState().transpose() << " ";
}
stream << ")" << std::endl;
}
else if (state_blocks)
{
stream << " sb: ";
for (auto& _key : S->getStructure())
{
auto key = std::string(1,_key);
auto sb = S->getStateBlock(key);
stream << key << "[" << (S->isStateBlockDynamic(key) ? "Dyn" : "Sta") << "," << (sb->isFixed() ? "Fix" : "Est") << "]; ";
}
stream << std::endl;
}
if (depth >= 2)
{
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (p->isMotion())
{
stream << " PrcM" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
if (pm->getOrigin())
stream << " o: Cap" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKeyOrAux() ? (pm->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm" ) : " Frm")
<< pm->getOrigin()->getFrame()->id() << std::endl;
if (pm->getLast())
stream << " l: Cap" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKeyOrAux() ? (pm->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pm->getLast()->getFrame()->id() << std::endl;
if (pm->getIncoming())
stream << " i: Cap" << pm->getIncoming()->id() << std::endl;
}
else
{
stream << " PrcT" << p->id() << " " << p->getType() << " \"" << p->getName() << "\"" << std::endl;
ProcessorTrackerPtr pt = std::dynamic_pointer_cast<ProcessorTracker>(p);
if (pt)
{
if (pt->getOrigin())
stream << " o: Cap" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKeyOrAux() ? (pt->getOrigin()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getOrigin()->getFrame()->id() << std::endl;
if (pt->getLast())
stream << " l: Cap" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKeyOrAux() ? (pt->getLast()->getFrame()->isKey() ? " KFrm" : " AFrm") : " Frm")
<< pt->getLast()->getFrame()->id() << std::endl;
if (pt->getIncoming())
stream << " i: Cap" << pt->getIncoming()->id() << std::endl;
}
}
} // for p
}
} // for S
}
stream << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << std::endl;
if (depth >= 1)
{
// Frames =======================================================================================
for (auto F : getTrajectory()->getFrameList())
{
stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " AFrm") : " Frm") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
{
stream << " <-- ";
for (auto cby : F->getConstrainedByList())
stream << "Fac" << cby->id() << " \t";
}
stream << std::endl;
if (metric)
{
stream << (F->isFixed() ? " Fix" : " Est") << ", ts=" << std::setprecision(5)
<< F->getTimeStamp();
stream << ",\t x = ( " << std::setprecision(2) << F->getState().transpose() << " )";
stream << std::endl;
}
if (state_blocks)
{
stream << " sb:";
for (const auto& sb : F->getStateBlockVec())
{
stream << " " << (sb->isFixed() ? "Fix" : "Est");
}
stream << std::endl;
}
if (depth >= 2)
{
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
stream << " Cap" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
if(C->getSensor() != nullptr)
{
stream << " -> Sen" << C->getSensor()->id();
}
else
stream << " -> Sen-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
if (auto OC = CM->getOriginCapture())
{
stream << " -> OCap" << OC->id();
if (auto OF = OC->getFrame())
stream << " ; OFrm" << OF->id();
}
}
stream << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
if (constr_by)
{
stream << " <-- ";
for (auto cby : C->getConstrainedByList())
stream << "Fac" << cby->id() << " \t";
}
stream << std::endl;
if (state_blocks)
for (const auto& sb : C->getStateBlockVec())
{
if(sb != nullptr)
{
stream << " sb: ";
stream << (sb->isFixed() ? "Fix" : "Est");
if (metric)
stream << std::setprecision(2) << " (" << sb->getState().transpose() << " )";
stream << std::endl;
}
}
if (C->isMotion() )
{
CaptureMotionPtr CM = std::dynamic_pointer_cast<CaptureMotion>(C);
stream << " buffer size : " << CM->getBuffer().get().size() << std::endl;
if ( metric && ! CM->getBuffer().get().empty())
{
stream << " delta preint : (" << CM->getDeltaPreint().transpose() << ")" << std::endl;
if (CM->hasCalibration())
{
stream << " calib preint : (" << CM->getCalibrationPreint().transpose() << ")" << std::endl;
stream << " jacob preint : (" << CM->getJacobianCalib().row(0) << ")" << std::endl;
stream << " calib current: (" << CM->getCalibration().transpose() << ")" << std::endl;
stream << " delta correct: (" << CM->getDeltaCorrected(CM->getCalibration()).transpose() << ")" << std::endl;
}
}
}
if (depth >= 3)
{
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
stream << " Ftr" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
if (constr_by)
{
stream << " <--\t";
for (auto cby : f->getConstrainedByList())
stream << "Fac" << cby->id() << " \t";
}
stream << std::endl;
if (metric)
stream << " m = ( " << std::setprecision(2) << f->getMeasurement().transpose()
<< " )" << std::endl;
if (depth >= 4)
{
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
stream << " Fac" << c->id() << " " << c->getType() << " -->";
if ( c->getFrameOtherList() .empty()
&& c->getCaptureOtherList() .empty()
&& c->getFeatureOtherList() .empty()
&& c->getLandmarkOtherList().empty())
stream << " Abs";
for (const auto& Fow : c->getFrameOtherList())
if (!Fow.expired())
stream << " Frm" << Fow.lock()->id();
for (const auto& Cow : c->getCaptureOtherList())
if (!Cow.expired())
stream << " Cap" << Cow.lock()->id();
for (const auto& fow : c->getFeatureOtherList())
if (!fow.expired())
stream << " Ftr" << fow.lock()->id();
for (const auto& Low : c->getLandmarkOtherList())
if (!Low.expired())
stream << " Lmk" << Low.lock()->id();
stream << std::endl;
} // for c
}
} // for f
}
} // for C
}
} // for F
}
stream << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << std::endl;
if (depth >= 1)
{
// Landmarks =======================================================================================
for (auto L : getMap()->getLandmarkList())
{
stream << " Lmk" << L->id() << " " << L->getType();
if (constr_by)
{
stream << "\t<-- ";
for (auto cby : L->getConstrainedByList())
stream << "Fac" << cby->id() << " \t";
}
stream << std::endl;
if (metric)
{
stream << (L->isFixed() ? " Fix" : " Est");
stream << ",\t x = ( " << std::setprecision(2) << L->getState().transpose() << " )";
stream << std::endl;
}
if (state_blocks)
{
stream << " sb:";
for (const auto& sb : L->getStateBlockVec())
{
if (sb != nullptr)
stream << (sb->isFixed() ? " Fix" : " Est");
}
stream << std::endl;
}
} // for L
}
stream << "-----------------------------------------" << std::endl;
stream << std::endl;
}
bool Problem::check(bool verbose, std::ostream& stream) const
{
CheckLog log(true, "");
log.explanation_ = "## WOLF::problem inconsistencies list \n ---------------------------------- \n";
if (verbose) stream << std::endl;
if (verbose) stream << "Wolf tree integrity ---------------------" << std::endl;
auto P_raw = this;
if (verbose)
{
stream << "Prb @ " << P_raw << std::endl;
}
// ------------------------
// HARDWARE branch
// ------------------------
auto H = hardware_ptr_;
if (verbose)
{
stream << "Hrw @ " << H.get() << std::endl;
}
// check pointer to Problem
std::stringstream inconsistency_explanation;
inconsistency_explanation << "Hardware problem pointer is " << H->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
log.addAssertion((H->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
// Sensors =======================================================================================
for (auto S : H->getSensorList())
{
if (verbose)
{
stream << " Sen" << S->id() << " @ " << S.get() << std::endl;
stream << " -> Prb @ " << S->getProblem().get() << std::endl;
stream << " -> Hrw @ " << S->getHardware().get() << std::endl;
for (auto pair: S->getStateBlockMap())
{
auto sb = pair.second;
stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
// check problem and hardware pointers
inconsistency_explanation << "Sensor problem pointer is " << S->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
log.addAssertion((S->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
inconsistency_explanation << "Sensor hardware pointer is " << S->getHardware()
<< " but hardware pointer is " << H << "\n";
log.addAssertion((S->getHardware() == H), inconsistency_explanation);
//Clear inconsistency_explanation
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose)
{
stream << " Prc" << p->id() << " @ " << p.get() << " -> Sen" << p->getSensor()->id() << std::endl;
stream << " -> Prb @ " << p->getProblem().get() << std::endl;
stream << " -> Sen" << p->getSensor()->id() << " @ " << p->getSensor().get() << std::endl;
}
// check problem and sensor pointers
inconsistency_explanation << "Processor problem pointer is " << p->getProblem().get()
<< " but problem pointer is " << P_raw << "\n";
log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
inconsistency_explanation << "Processor sensor pointer is " << p->getSensor()
<< " but sensor pointer is " << P_raw << "\n";
log.addAssertion((p->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
}
}
// ------------------------
// TRAJECTORY branch
// ------------------------
auto T = trajectory_ptr_;
if (verbose)
{
stream << "Trj @ " << T.get() << std::endl;
}
// check pointer to Problem
inconsistency_explanation << "Trajectory problem pointer is " << T->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
log.addAssertion((T->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
// Frames =======================================================================================
for (auto F : T->getFrameList())
{
if (verbose) {
stream << (F->isKeyOrAux() ? (F->isKey() ? " KFrm" : " EFrm") : " Frm")
<< F->id() << " @ " << F.get() << std::endl;
stream << " -> Prb @ " << F->getProblem().get() << std::endl;
stream << " -> Trj @ " << F->getTrajectory().get() << std::endl;
}
for (const auto &pair: F->getStateBlockMap()) {
auto sb = pair.second;
// check for valid state block
inconsistency_explanation << "Frame " << F->id() << " @ "<< F.get()
<< " has State block pointer " << sb.get()
<< " = 0 \n";
log.addAssertion((sb.get() != 0), inconsistency_explanation);
if (verbose) {
stream << " "<< pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
// check problem pointer
inconsistency_explanation << "Frame problem pointer is " << F->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
log.addAssertion((F->getProblem().get() == P_raw), inconsistency_explanation);
// check trajectory pointer
inconsistency_explanation << "Frame trajectory pointer is " << F->getTrajectory()
<< " but trajectory pointer is" << T << "\n";
log.addAssertion((F->getTrajectory() == T), inconsistency_explanation);
//Clear inconsistency_explanation
// check constrained_by
for (auto cby : F->getConstrainedByList())
{
if (verbose)
{
stream << " <- c" << cby->id() << " -> ";
for (const auto& Fow : cby->getFrameOtherList())
stream << " F" << Fow.lock()->id() << std::endl;
}
// check constrained_by pointer to this frame
inconsistency_explanation << "constrained-by frame " << F->id() << " @ " << F
<< " not found among constrained-by factors\n";
log.addAssertion((cby->hasFrameOther(F)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose) {
stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
}
// Captures =======================================================================================
for (auto C : F->getCaptureList())
{
if (verbose)
{
stream << " Cap" << C->id() << " @ " << C.get() << " -> Sen";
if (C->getSensor()) stream << C->getSensor()->id();
else stream << "-";
stream << std::endl;
stream << " -> Prb @ " << C->getProblem().get() << std::endl;
stream << " -> Frm" << C->getFrame()->id() << " @ " << C->getFrame().get() << std::endl;
}
for (auto pair: C->getStateBlockMap())
{
auto sb = pair.second;
// check for valid state block
inconsistency_explanation << "Capture " << C->id() << " @ " << C.get() << " has State block pointer "
<< sb.get() << " = 0 \n";
log.addAssertion((sb.get() != 0), inconsistency_explanation);
if (verbose)
{
stream << " " << pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
// check problem and frame pointers
inconsistency_explanation << "Capture problem pointer is " << C->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
log.addAssertion((C->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
inconsistency_explanation << "Capture frame pointer is " << C->getFrame()
<< " but frame pointer is" << F << "\n";
log.addAssertion((C->getFrame() == F), inconsistency_explanation);
//Clear inconsistency_explanation
// check contrained_by
for (const auto& cby : C->getConstrainedByList())
{
if (verbose)
{
stream << " <- c" << cby->id() << " -> ";
for (const auto& Cow : cby->getCaptureOtherList())
stream << " C" << Cow.lock()->id();
stream << std::endl;
}
// check constrained_by pointer to this capture
inconsistency_explanation << "constrained by capture " << C->id() << " @ " << C
<< " not found among constrained-by factors\n";
log.addAssertion((cby->hasCaptureOther(C)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose)
{
stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
}
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
if (verbose)
{
stream << " Ftr" << f->id() << " @ " << f.get() << std::endl;
stream << " -> Prb @ " << f->getProblem().get() << std::endl;
stream << " -> Cap" << f->getCapture()->id() << " @ " << f->getCapture().get()
<< std::endl;
}
// check problem and capture pointers
inconsistency_explanation << "Feature frame pointer is " << f->getProblem().get()
<< " but problem pointer is" << P_raw << "\n";
log.addAssertion((f->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
inconsistency_explanation << "Feature capture pointer is " << f->getCapture()
<< " but capture pointer is" << C << "\n";
log.addAssertion((f->getCapture() == C), inconsistency_explanation);
//Clear inconsistency_explanation
// check contrained_by
for (auto cby : f->getConstrainedByList())
{
if (verbose)
{
stream << " <- c" << cby->id() << " -> ";
for (const auto& fow : cby->getFeatureOtherList())
stream << " f" << fow.lock()->id();
stream << std::endl;
}
// check constrained_by pointer to this feature
inconsistency_explanation << "constrained by Feature " << f->id() << " @ " << f
<< " not found among constrained-by factors\n";
log.addAssertion((cby->hasFeatureOther(f)), inconsistency_explanation);
//Clear inconsistency_explanation
}
// Factors =======================================================================================
for (auto c : f->getFactorList())
{
if (verbose)
stream << " Fac" << c->id() << " @ " << c.get();
if ( c->getFrameOtherList() .empty()
&& c->getCaptureOtherList() .empty()
&& c->getFeatureOtherList() .empty()
&& c->getLandmarkOtherList().empty() ) // case ABSOLUTE:
{
if (verbose)
stream << " --> Abs.";
}
// find constrained_by pointer in constrained frame
for (const auto& Fow : c->getFrameOtherList())
{
if (!Fow.expired())
{
const auto& Fo = Fow.lock();
if (verbose)
{
stream << " ( --> F" << Fo->id() << " <- ";
for (auto cby : Fo->getConstrainedByList())
stream << " c" << cby->id();
}
// check constrained_by pointer in constrained frame
bool found = Fo->isConstrainedBy(c);
inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
<< " not found among constrained-by factors\n";
log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
}
}
if (verbose && !c->getFrameOtherList().empty())
stream << ")";
// find constrained_by pointer in constrained capture
for (const auto& Cow : c->getCaptureOtherList())
{
if (!Cow.expired())
{
const auto& Co = Cow.lock();
if (verbose)
{
stream << " ( --> C" << Co->id() << " <- ";
for (auto cby : Co->getConstrainedByList())
stream << " c" << cby->id();
}
// check constrained_by pointer in constrained frame
bool found = Co->isConstrainedBy(c);
inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
<< " not found among constrained-by factors\n";
log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
}
}
if (verbose && !c->getCaptureOtherList().empty())
stream << ")";
// find constrained_by pointer in constrained feature
for (const auto& fow : c->getFeatureOtherList())
{
if (!fow.expired())
{
const auto& fo = fow.lock();
if (verbose)
{
stream << " ( --> f" << fo->id() << " <- ";
for (auto cby : fo->getConstrainedByList())
stream << " c" << cby->id();
}
// check constrained_by pointer in constrained feature
bool found = fo->isConstrainedBy(c);
inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
<< " not found among constrained-by factors\n";
log.addAssertion((found), inconsistency_explanation);
}
}
if (verbose && !c->getFeatureOtherList().empty())
stream << ")";
// find constrained_by pointer in constrained landmark
for (const auto& Low : c->getLandmarkOtherList())
{
if (Low.expired())
{
const auto& Lo = Low.lock();
if (verbose)
{
stream << " ( --> L" << Lo->id() << " <- ";
for (auto cby : Lo->getConstrainedByList())
stream << " c" << cby->id();
}
// check constrained_by pointer in constrained landmark
bool found = Lo->isConstrainedBy(c);
inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
<< " not found among constrained-by factors\n";
log.addAssertion((found), inconsistency_explanation);
//Clear inconsistency_explanation
}
}
if (verbose && !c->getLandmarkOtherList().empty())
stream << ")";
if (verbose)
stream << std::endl;
if (verbose)
{
stream << " -> Prb @ " << c->getProblem().get() << std::endl;
stream << " -> Ftr" << c->getFeature()->id() << " @ " << c->getFeature().get() << std::endl;
}
// check problem and feature pointers
inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has problem ptr " << c->getProblem().get()
<< " but problem ptr is " << P_raw << "\n";
log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
inconsistency_explanation << "The factor " << c->id() << " @ " << c << " has feature ptr " << c->getFeature()
<< " but it should have " << f << "\n";
log.addAssertion((c->getProblem().get() == P_raw), inconsistency_explanation);
//Clear inconsistency_explanation
// find state block pointers in all constrained nodes
SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
if (verbose)
{
stream << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
}
bool found_here;
// find in own Frame
found_here = F->hasStateBlock(sb);
if (found_here && verbose) stream << " F" << F->id();
found = found || found_here;
// find in own Capture
found_here = C->hasStateBlock(sb);
if (found_here && verbose) stream << " C" << C->id();
found = found || found_here;
// Find in other Captures of the own Frame
if (!found_here)
for (auto FC : F->getCaptureList())
{
found_here = FC->hasStateBlock(sb);
if (found_here && verbose) stream << " F" << F->id() << ".C" << FC->id();
found = found || found_here;
}
// find in own Sensor
if (S)
{
found_here = S->hasStateBlock(sb);
if (found_here && verbose) stream << " S" << S->id();
found = found || found_here;
}
// find in constrained Frame
for (const auto& Fow : c->getFrameOtherList())
{
if (!Fow.expired())
{
const auto& Fo = Fow.lock();
found_here = Fo->hasStateBlock(sb);
if (found_here && verbose) stream << " Fo" << Fo->id();
found = found || found_here;
// find in feature other's captures
for (auto FoC : Fo->getCaptureList())
{
found_here = FoC->hasStateBlock(sb);
if (found_here && verbose) stream << " Fo" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
}
}
// find in constrained Capture
for (const auto& Cow : c->getCaptureOtherList())
{
if (!Cow.expired())
{
const auto& Co = Cow.lock();
found_here = Co->hasStateBlock(sb);
if (found_here && verbose) stream << " Co" << Co->id();
found = found || found_here;
}
}
// find in constrained Feature
for (const auto& fow : c->getFeatureOtherList())
{
if (!fow.expired())
{
const auto& fo = fow.lock();
// find in constrained feature's Frame
auto foF = fo->getFrame();
found_here = foF->hasStateBlock(sb);
if (found_here && verbose) stream << " foF" << foF->id();
found = found || found_here;
// find in constrained feature's Capture
auto foC = fo->getCapture();
found_here = foC->hasStateBlock(sb);
if (found_here && verbose) stream << " foC" << foC->id();
found = found || found_here;
// find in constrained feature's Sensor
auto foS = fo->getCapture()->getSensor();
found_here = foS->hasStateBlock(sb);
if (found_here && verbose) stream << " foS" << foS->id();
found = found || found_here;
}
}
// find in constrained landmark
for (const auto& Low : c->getLandmarkOtherList())
{
if (!Low.expired())
{
const auto& Lo = Low.lock();
found_here = Lo->hasStateBlock(sb);
if (found_here && verbose) stream << " Lo" << Lo->id();
found = found || found_here;
}
}
if (verbose)
{
if (found)
stream << " found";
else
stream << " NOT FOUND !";
stream << std::endl;
}
// check that the state block has been found somewhere
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
log.addAssertion((found), inconsistency_explanation);
inconsistency_explanation << "The stateblock " << sb << " of factor " << c->id() << " @ " << c << " is null\n";
log.addAssertion((sb.get() != nullptr), inconsistency_explanation);
}
}
}
}
}
// ------------------------
// MAP branch
// ------------------------
auto M = map_ptr_;
if (verbose)
stream << "Map @ " << M.get() << std::endl;
// check pointer to Problem
inconsistency_explanation << "The map problem ptr is " << M->getProblem().get() << " but problem is " << P_raw << "\n";
log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
// Landmarks =======================================================================================
for (auto L : M->getLandmarkList())
{
if (verbose)
{
stream << " Lmk" << L->id() << " @ " << L.get() << std::endl;
stream << " -> Prb @ " << L->getProblem().get() << std::endl;
stream << " -> Map @ " << L->getMap().get() << std::endl;
for (const auto& pair : L->getStateBlockMap())
{
auto sb = pair.second;
stream << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
// check problem and map pointers
inconsistency_explanation << "Landmarks' problem ptr is "
<< L->getProblem().get() << " but problem is "
<< P_raw << "\n";
log.addAssertion((L->getProblem().get() == P_raw), inconsistency_explanation);
inconsistency_explanation << "The Landmarks' map ptr is "
<< L->getMap() << " but map is "
<< M << "\n";
log.addAssertion((M->getProblem().get() == P_raw), inconsistency_explanation);
for (auto cby : L->getConstrainedByList())
{
if (verbose)
{
stream << " <- Fac" << cby->id() << " ->";
for (const auto& Low : cby->getLandmarkOtherList())
{
if (!Low.expired())
{
const auto& Lo = Low.lock();
stream << " Lmk" << Lo->id();
}
}
stream << std::endl;
}
// check constrained-by factors
inconsistency_explanation << "constrained-by landmark " << L->id() << " @ " << L
<< " not found among constrained-by factors\n";
log.addAssertion((cby->hasLandmarkOther(L)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector()) {
if (verbose) {
stream << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
stream << " (lp @ " << lp.get() << ")";
}
stream << std::endl;
}
}
}
}
if (verbose) stream << "--------------------------- Wolf tree " << (log.is_consistent_ ? " OK" : "Not OK !!") << std::endl;
if (verbose) stream << std::endl;
if (verbose and not log.is_consistent_) stream << log.explanation_ << std::endl;
return log.is_consistent_;
}
bool Problem::check(int verbose_level) const
{
return check((verbose_level > 0), std::cout);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks) const
{
print(depth, std::cout, constr_by, metric, state_blocks);
}
void Problem::print(const std::string& depth, bool constr_by, bool metric, bool state_blocks) const
{
if (depth.compare("T") == 0)
print(0, constr_by, metric, state_blocks);
else if (depth.compare("F") == 0)
print(1, constr_by, metric, state_blocks);
else if (depth.compare("C") == 0)
print(2, constr_by, metric, state_blocks);
else if (depth.compare("f") == 0)
print(3, constr_by, metric, state_blocks);
else if (depth.compare("c") == 0)
print(4, constr_by, metric, state_blocks);
else
print(0, constr_by, metric, state_blocks);
}
void Problem::perturb(double amplitude)
{
// Sensors
for (auto& S : getHardware()->getSensorList())
S->perturb(amplitude);
// Frames and Captures
for (auto& F : getTrajectory()->getFrameList())
{
F->perturb(amplitude);
for (auto& C : F->getCaptureList())
C->perturb(amplitude);
}
// Landmarks
for (auto& L : getMap()->getLandmarkList())
L->perturb(amplitude);
}
} // namespace wolf