Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
View open merge request
Hide whitespace changes
Inline Side-by-side
Showing
with 2097 additions and 1743 deletions
src/capture/capture_pose.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/capture/capture_pose.h"
namespace wolf{
namespace wolf
{
CapturePose::CapturePose(const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
const Eigen::VectorXd& _data,
const Eigen::MatrixXd& _data_covariance)
: CaptureBase("CapturePose", _ts, _sensor_ptr), data_(_data), data_covariance_(_data_covariance)
{
assert(_data.size() == 3 or _data.size() == 7);
assert(_data_covariance.rows() == _data_covariance.cols());
assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
}
CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance) :
CaptureBase("CapturePose", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
CapturePose::~CapturePose()
{
//
}
CapturePose::~CapturePose()
void CapturePose::setData(const Eigen::VectorXd& _data)
{
assert(_data.size() == 3 or _data.size() == 7);
data_ = _data;
}
void CapturePose::setDataCovariance(const Eigen::MatrixXd& _data_covariance)
{
//
assert(_data_covariance.rows() == _data_covariance.cols());
assert(_data_covariance.rows() == 3 or _data_covariance.rows() == 6);
data_covariance_ = _data_covariance;
}
void CapturePose::emplaceFeatureAndFactor()
void CapturePose::emplaceFeatureAndFactor(const ProcessorBasePtr& _processor, bool _apply_loss_function)
{
// Emplace feature
// FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
// addFeature(feature_pose);
auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this(), data_, data_covariance_);
auto feature_pose = FeatureBase::emplace<FeaturePose>(shared_from_this_capture(), data_, data_covariance_);
// std::cout << data_.size() << " ~~ " << data_covariance_.rows() << "x" << data_covariance_.cols() << std::endl;
// Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
FactorBase::emplace<FactorPose2d>(feature_pose, feature_pose, nullptr, false);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
FactorBase::emplace<FactorPose3d>(feature_pose, feature_pose, nullptr, false);
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3)
FactorBase::emplace<FactorPose2d>(feature_pose,
feature_pose->getMeasurement(),
feature_pose->getMeasurementSquareRootInformationUpper(),
getFrame(),
_processor,
_apply_loss_function);
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6)
FactorBase::emplace<FactorPose3d>(feature_pose,
feature_pose->getMeasurement(),
feature_pose->getMeasurementSquareRootInformationUpper(),
getFrame(),
_processor,
_apply_loss_function);
else
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2d. Use 7 for 3d.");
}
} // namespace wolf
} // namespace wolf
src/capture/capture_void.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/capture/capture_void.h"
namespace wolf {
CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr) :
CaptureBase("CaptureVoid", _ts, _sensor_ptr)
namespace wolf
{
CaptureVoid::CaptureVoid(const TimeStamp& _ts, SensorBasePtr _sensor_ptr)
: CaptureBase("CaptureVoid", _ts, _sensor_ptr)
{
//
}
CaptureVoid::~CaptureVoid()
{
//std::cout << "deleting CaptureVoid " << id() << std::endl;
// std::cout << "deleting CaptureVoid " << id() << std::endl;
}
} // namespace wolf
} // namespace wolf
src/ceres_wrapper/local_parametrization_wrapper.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/ceres_wrapper/local_parametrization_wrapper.h"
namespace wolf {
namespace wolf
{
bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
{
Eigen::Map<const Eigen::VectorXd> x_raw_map((double*)x_raw, GlobalSize());
Eigen::Map<const Eigen::VectorXd> delta_raw_map((double*)delta_raw, LocalSize());
Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map);
};
bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const
{
Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
Eigen::Map<Eigen::MatrixRowXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
};
} // namespace wolf
} // namespace wolf
src/ceres_wrapper/qr_manager.cpp deleted 100644 → 0
View file @ c7bb5751
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* qr_manager.cpp
*
* Created on: Jun 7, 2017
* Author: jvallve
*/
#include "qr_manager.h"
namespace wolf {
QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
SolverManager(_wolf_problem),
A_(), // empty matrix
b_(),
any_state_block_removed_(false),
new_state_blocks_(0),
N_batch_(_N_batch),
pending_changes_(false)
{
//
}
QRManager::~QRManager()
{
sb_2_col_.clear();
fac_2_row_.clear();
}
std::string QRManager::solve(const unsigned int& _report_level)
{
// check for update notifications
update();
// Decomposition
if (!computeDecomposition())
return std::string("decomposition failed\n");
// Solve
Eigen::VectorXd x_incr = solver_.solve(-b_);
b_.setZero();
// update state blocks
for (auto sb_pair : sb_2_col_)
sb_pair.first->setState(sb_pair.first->getState() + x_incr.segment(sb_pair.second, sb_pair.first->getSize()), false);
if (_report_level == 1)
return std::string("Success!\n");
else if (_report_level == 2)
return std::string("Success!\n");
return std::string();
}
void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
{
// TODO
}
void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
{
//std::cout << "computing covariances.." << std::endl;
update();
computeDecomposition();
// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
// std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl;
covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(),A_.cols());
I.setIdentity();
Eigen::SparseMatrix<double, Eigen::ColMajor> iR = covariance_solver_.solve(I);
Eigen::MatrixXd Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
// std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl;
// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl;
// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl;
// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
// std::cout << "Sigma = \n" << Sigma_full << std::endl;
// STORE DESIRED COVARIANCES
for (auto sb_row = _sb_list.begin(); sb_row != _sb_list.end(); sb_row++)
for (auto sb_col = sb_row; sb_col!=_sb_list.end(); sb_col++)
{
//std::cout << "cov state block " << sb_col->get() << std::endl;
assert(sb_2_col_.find(*sb_col) != sb_2_col_.end() && "state block not found");
//std::cout << "block: " << sb_2_col_[*sb_row] << "," << sb_2_col_[*sb_col] << std::endl << Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()) << std::endl;
wolf_problem_->addCovarianceBlock(*sb_row, *sb_col, Sigma_full.block(sb_2_col_[*sb_row], sb_2_col_[*sb_col], (*sb_row)->getSize(), (*sb_col)->getSize()));
}
}
bool QRManager::computeDecomposition()
{
if (pending_changes_)
{
// Rebuild problem
if (any_state_block_removed_)
{
// rebuild maps
unsigned int state_size = 0;
for (auto sb_pair : sb_2_col_)
{
sb_2_col_[sb_pair.first] = state_size;
state_size += sb_pair.first->getSize();
}
unsigned int meas_size = 0;
for (auto fac_pair : fac_2_row_)
{
fac_2_row_[fac_pair.first] = meas_size;
meas_size += fac_pair.first->getSize();
}
// resize and setZero A, b
A_.resize(meas_size,state_size);
b_.resize(meas_size);
}
if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
{
// relinearize all factors
for (auto fac_pair : fac_2_row_)
relinearizeFactor(fac_pair.first);
any_state_block_removed_ = false;
new_state_blocks_ = 0;
}
// Decomposition
solver_.compute(A_);
if (solver_.info() != Eigen::Success)
return false;
}
pending_changes_ = false;
return true;
}
void QRManager::addFactor(FactorBasePtr _fac_ptr)
{
//std::cout << "add factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
fac_2_row_[_fac_ptr] = A_.rows();
A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols());
b_.conservativeResize(b_.size() + _fac_ptr->getSize());
assert(A_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad A number of rows");
assert(b_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad b number of rows");
relinearizeFactor(_fac_ptr);
pending_changes_ = true;
}
void QRManager::removeFactor(FactorBasePtr _fac_ptr)
{
//std::cout << "remove factor " << _fac_ptr->id() << std::endl;
assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize());
b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero();
fac_2_row_.erase(_fac_ptr);
pending_changes_ = true;
}
void QRManager::addStateBlock(StateBlockPtr _st_ptr)
{
//std::cout << "add state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) == sb_2_col_.end() && "adding existing state block");
sb_2_col_[_st_ptr] = A_.cols();
A_.conservativeResize(A_.rows(), A_.cols() + _st_ptr->getSize());
new_state_blocks_++;
pending_changes_ = true;
}
void QRManager::removeStateBlock(StateBlockPtr _st_ptr)
{
//std::cout << "remove state block " << _st_ptr.get() << std::endl;
assert(sb_2_col_.find(_st_ptr) != sb_2_col_.end() && "removing unknown state block");
eraseBlockCol(A_, sb_2_col_[_st_ptr], _st_ptr->getSize());
// flag to rebuild problem
any_state_block_removed_ = true;
// TODO: insert identity while problem is not re-built?
sb_2_col_.erase(_st_ptr);
pending_changes_ = true;
}
void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
{
//std::cout << "update state block " << _st_ptr.get() << std::endl;
if (_st_ptr->isFixed())
{
if (sb_2_col_.find(_st_ptr) != sb_2_col_.end())
removeStateBlock(_st_ptr);
}
else
if (sb_2_col_.find(_st_ptr) == sb_2_col_.end())
addStateBlock(_st_ptr);
}
void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
// evaluate factor
std::vector<const double*> fac_states_ptr;
for (auto sb : _fac_ptr->getStateBlockPtrVector())
fac_states_ptr.push_back(sb->get());
Eigen::VectorXd residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXd> jacobians;
_fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
// Fill jacobians
Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
}
assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
// Fill residual
b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
}
} /* namespace wolf */
src/ceres_wrapper/solver_ceres.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/ceres_wrapper/solver_ceres.h"
#include "core/ceres_wrapper/create_numeric_diff_cost_function.h"
#include "core/ceres_wrapper/cost_function_wrapper.h"
......@@ -30,35 +27,80 @@
namespace wolf
{
SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem) :
SolverCeres(_wolf_problem, std::make_shared<ParamsCeres>())
SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params)
: SolverManager(_wolf_problem, _params),
n_iter_acc_(0),
n_iter_max_(0),
n_convergence_(0),
n_interrupted_(0),
n_no_convergence_(0)
{
}
SolverCeres::SolverCeres(const ProblemPtr& _wolf_problem,
const ParamsCeresPtr& _params)
: SolverManager(_wolf_problem, _params)
, params_ceres_(_params)
, n_iter_acc_(0)
, n_iter_max_(0)
, n_convergence_(0)
, n_interrupted_(0)
, n_no_convergence_(0)
{
covariance_ = std::make_unique<ceres::Covariance>(params_ceres_->covariance_options);
ceres_problem_ = std::make_unique<ceres::Problem>(params_ceres_->problem_options);
if (params_ceres_->interrupt_on_problem_change)
getSolverOptions().callbacks.push_back(new IterationUpdateCallback(wolf_problem_,
params_ceres_->min_num_iterations,
params_ceres_->verbose != SolverManager::ReportVerbosity::QUIET));
// LOAD PARAMETERS ======================================================
// CERES SOLVER OPTIONS
solver_options_ = ceres::Solver::Options();
solver_options_.max_num_iterations = _params["max_num_iterations"].as<int>();
solver_options_.parameter_tolerance = _params["parameter_tolerance"].as<double>();
solver_options_.function_tolerance = _params["function_tolerance"].as<double>();
solver_options_.gradient_tolerance = _params["gradient_tolerance"].as<double>();
solver_options_.num_threads = _params["n_threads"].as<int>();
std::string minimizer = _params["minimizer"].as<std::string>();
if (minimizer == "levenberg_marquardt")
{
solver_options_.minimizer_type = ceres::TRUST_REGION;
solver_options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
}
else if (minimizer == "dogleg")
{
solver_options_.minimizer_type = ceres::TRUST_REGION;
solver_options_.trust_region_strategy_type = ceres::DOGLEG;
}
else if (minimizer == "lbfgs")
{
solver_options_.minimizer_type = ceres::LINE_SEARCH;
solver_options_.line_search_direction_type = ceres::LBFGS;
}
else if (minimizer == "bfgs")
{
solver_options_.minimizer_type = ceres::LINE_SEARCH;
solver_options_.line_search_direction_type = ceres::BFGS;
}
else
{
throw std::runtime_error(
"ParamsCeres: Wrong parameter 'minimizer'. Should be 'levenberg_marquardt', 'dogleg', 'lbfgs' or 'bfgs'");
}
if (solver_options_.minimizer_type == ceres::TRUST_REGION) // specific options for TRUST REGION
{
solver_options_.use_nonmonotonic_steps = _params["use_nonmonotonic_steps"].as<bool>();
if (solver_options_.use_nonmonotonic_steps)
solver_options_.max_consecutive_nonmonotonic_steps =
_params["max_consecutive_nonmonotonic_steps"].as<int>();
}
// interrupt solver whenever the problem is updated (via ceres::iterationCallback)
if (_params["interrupt_on_problem_change"].as<bool>())
solver_options_.callbacks.push_back(
new IterationUpdateCallback(wolf_problem_,
_params["min_num_iterations"].as<int>(),
verbose_ != SolverManager::ReportVerbosity::QUIET));
// PROBLEM OPTIONS
auto problem_options = ceres::Problem::Options();
problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
problem_options.loss_function_ownership = ceres::TAKE_OWNERSHIP;
problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
ceres_problem_ = std::make_unique<ceres::Problem>(problem_options);
// COVARIANCE OPTIONS
auto covariance_options = ceres::Covariance::Options();
covariance_options.apply_loss_function = false;
covariance_options.num_threads = solver_options_.num_threads;
covariance_ = std::make_unique<ceres::Covariance>(covariance_options);
}
SolverCeres::~SolverCeres()
{
while (!fac_2_residual_idx_.empty())
removeFactorDerived(fac_2_residual_idx_.begin()->first);
while (!fac_2_residual_idx_.empty()) removeFactorDerived(fac_2_residual_idx_.begin()->first);
while (!state_blocks_.empty())
{
......@@ -66,21 +108,21 @@ SolverCeres::~SolverCeres()
state_blocks_.erase(state_blocks_.begin());
}
while (!getSolverOptions().callbacks.empty())
while (!solver_options_.callbacks.empty())
{
delete getSolverOptions().callbacks.back();
getSolverOptions().callbacks.pop_back();
delete solver_options_.callbacks.back();
solver_options_.callbacks.pop_back();
}
}
std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
{
// run Ceres Solver
ceres::Solve(getSolverOptions(), ceres_problem_.get(), &summary_);
ceres::Solve(solver_options_, ceres_problem_.get(), &summary_);
std::string report;
//return report
// return report
if (report_level == ReportVerbosity::BRIEF)
report = summary_.BriefReport();
else if (report_level == ReportVerbosity::FULL)
......@@ -92,7 +134,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
n_iter_max_ = std::max(n_iter_max_, iterations());
// convergence (profiling)
if (hasConverged())
if (converged())
n_convergence_++;
else if (wasStopped())
n_interrupted_++;
......@@ -103,7 +145,7 @@ std::string SolverCeres::solveDerived(const ReportVerbosity report_level)
}
bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _blocks)
{
{
// update problem
update();
......@@ -113,86 +155,81 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
switch (_blocks)
{
case CovarianceBlocksToBeComputed::ALL:
{
case CovarianceBlocksToBeComputed::ALL: {
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key : fr_pair.second->getStructure())
// frame state blocks
for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key : fr_pair.second->getKeys())
{
const auto& sb = fr_pair.second->getStateBlock(key);
all_state_blocks.push_back(sb);
}
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getStructure())
for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getKeys())
{
const auto& sb = l_ptr->getStateBlock(key);
all_state_blocks.push_back(sb);
}
// double loop all against all (without repetitions)
for (unsigned int i = 0; i < all_state_blocks.size(); i++)
{
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
for (unsigned int j = i; j < all_state_blocks.size(); j++)
for (unsigned int j = i; j < all_state_blocks.size(); j++)
{
assert(isStateBlockRegisteredDerived(all_state_blocks[i]));
state_block_pairs.emplace_back(all_state_blocks[i],all_state_blocks[j]);
state_block_pairs.emplace_back(all_state_blocks[i], all_state_blocks[j]);
double_pairs.emplace_back(getAssociatedMemBlockPtr(all_state_blocks[i]),
getAssociatedMemBlockPtr(all_state_blocks[j]));
}
}
break;
}
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
case CovarianceBlocksToBeComputed::ALL_MARGINALS: {
// first create a vector containing all state blocks
for(auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key1 : fr_pair.second->getStructure())
for (auto fr_pair : wolf_problem_->getTrajectory()->getFrameMap())
for (auto key1 : fr_pair.second->getKeys())
{
const auto& sb1 = fr_pair.second->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
for (auto key2 : fr_pair.second->getStructure())
for (auto key2 : fr_pair.second->getKeys())
{
const auto& sb2 = fr_pair.second->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb1, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
if (sb1 == sb2)
break;
if (sb1 == sb2) break;
}
}
// landmark state blocks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key1 : l_ptr->getStructure())
for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key1 : l_ptr->getKeys())
{
const auto& sb1 = l_ptr->getStateBlock(key1);
assert(isStateBlockRegisteredDerived(sb1));
for (auto key2 : l_ptr->getStructure())
for (auto key2 : l_ptr->getKeys())
{
const auto& sb2 = l_ptr->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb1, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb1), getAssociatedMemBlockPtr(sb2));
if (sb1 == sb2)
break;
if (sb1 == sb2) break;
}
}
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
{
//robot-robot
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS: {
// robot-robot
auto last_key_frame = wolf_problem_->getLastFrame();
assert(isStateBlockRegisteredDerived(last_key_frame->getP()));
......@@ -210,8 +247,8 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getStructure())
for (auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto key : l_ptr->getKeys())
{
const auto& sb = l_ptr->getStateBlock(key);
assert(isStateBlockRegisteredDerived(sb));
......@@ -225,187 +262,188 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
getAssociatedMemBlockPtr(sb));
// landmark marginal
for (auto key2 : l_ptr->getStructure())
for (auto key2 : l_ptr->getKeys())
{
const auto& sb2 = l_ptr->getStateBlock(key2);
assert(isStateBlockRegisteredDerived(sb2));
state_block_pairs.emplace_back(sb, sb2);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb), getAssociatedMemBlockPtr(sb2));
if (sb == sb2)
break;
if (sb == sb2) break;
}
}
break;
}
case CovarianceBlocksToBeComputed::GAUSS:
{
case CovarianceBlocksToBeComputed::GAUSS: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
if (not frame)
return false;
if (not frame) return false;
while (frame)
{
if (frame->hasStateBlock('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
frame->hasStateBlock('V') and
isStateBlockRegisteredDerived(frame->getStateBlock('V')))
if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V')))
{
break;
}
//else
WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
// else
WOLF_DEBUG("Frame ",
frame->id(),
" hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and V, returning...");
WOLF_WARN("Couldn't find a frame with valid and registered state blocks P and V, returning...");
return false;
}
// only marginals of P and V
WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
"\nP @", frame->getStateBlock('P'),
"\nV @", frame->getStateBlock('V'));
state_block_pairs.emplace_back(frame->getStateBlock('P'),
frame->getStateBlock('P'));
WOLF_DEBUG("Storing pairs of frame ",
frame->id(),
" with state blocks: "
"\nP @",
frame->getStateBlock('P'),
"\nV @",
frame->getStateBlock('V'));
state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
state_block_pairs.emplace_back(frame->getStateBlock('V'),
frame->getStateBlock('V'));
state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
getAssociatedMemBlockPtr(frame->getStateBlock('V')));
break;
}
case CovarianceBlocksToBeComputed::GAUSS_TUCAN:
{
case CovarianceBlocksToBeComputed::GAUSS_TUCAN: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
if (not frame)
return false;
if (not frame) return false;
StateBlockPtr sb_gnss_T;
while (frame)
{
// get capture gnss
for (CaptureBasePtr cap : frame->getCaptureList())
if (cap->hasStateBlock('T'))
if (cap->has('T'))
{
sb_gnss_T = cap->getStateBlock('T');
break;
}
if (frame->hasStateBlock('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
frame->hasStateBlock('O') and
isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
frame->hasStateBlock('V') and
isStateBlockRegisteredDerived(frame->getStateBlock('V')) and
sb_gnss_T and
if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
frame->has('O') and isStateBlockRegisteredDerived(frame->getStateBlock('O')) and
frame->has('V') and isStateBlockRegisteredDerived(frame->getStateBlock('V')) and sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
{
break;
}
// else
WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
WOLF_DEBUG("Frame ",
frame->id(),
" hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P, O, V and CaptureGnss with T, returning...");
WOLF_WARN(
"Couldn't find a frame with valid and registered state blocks P, "
"O, V and CaptureGnss with T, returning...");
return false;
}
// only marginals of P, O, V and T
WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
"\nP @", frame->getStateBlock('P'),
"\nO @", frame->getStateBlock('O'),
"\nV @", frame->getStateBlock('V'),
"\nT @", sb_gnss_T);
state_block_pairs.emplace_back(frame->getStateBlock('P'),
frame->getStateBlock('P'));
WOLF_DEBUG("storing pairs of frame ",
frame->id(),
" with state blocks: "
"\nP @",
frame->getStateBlock('P'),
"\nO @",
frame->getStateBlock('O'),
"\nV @",
frame->getStateBlock('V'),
"\nT @",
sb_gnss_T);
state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
state_block_pairs.emplace_back(frame->getStateBlock('O'),
frame->getStateBlock('O'));
state_block_pairs.emplace_back(frame->getStateBlock('O'), frame->getStateBlock('O'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('O')),
getAssociatedMemBlockPtr(frame->getStateBlock('O')));
state_block_pairs.emplace_back(frame->getStateBlock('V'),
frame->getStateBlock('V'));
state_block_pairs.emplace_back(frame->getStateBlock('V'), frame->getStateBlock('V'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('V')),
getAssociatedMemBlockPtr(frame->getStateBlock('V')));
state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
getAssociatedMemBlockPtr(sb_gnss_T));
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T));
break;
}
case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION:
{
case CovarianceBlocksToBeComputed::GAUSS_TUCAN_ONLY_POSITION: {
// State blocks:
// - Last KF: PV
// last KF
FrameBasePtr frame = wolf_problem_->getLastFrame();
if (not frame)
return false;
if (not frame) return false;
StateBlockPtr sb_gnss_T;
while (frame)
{
// get capture gnss
for (CaptureBasePtr cap : frame->getCaptureList())
if (cap->hasStateBlock('T'))
if (cap->has('T'))
{
sb_gnss_T = cap->getStateBlock('T');
break;
}
if (frame->hasStateBlock('P') and
isStateBlockRegisteredDerived(frame->getStateBlock('P')) and
sb_gnss_T and
if (frame->has('P') and isStateBlockRegisteredDerived(frame->getStateBlock('P')) and sb_gnss_T and
isStateBlockRegisteredDerived(sb_gnss_T))
{
break;
}
// else
WOLF_DEBUG("SolverCeres::computeCovariances: Frame ", frame->id(), " hasn't the state blocks or they are not registered. Trying with the previous one...");
WOLF_DEBUG("Frame ",
frame->id(),
" hasn't the state blocks or they are not registered. Trying with the previous one...");
frame = frame->getPreviousFrame();
}
if (not frame)
{
WOLF_WARN("SolverCeres::computeCovariances: Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, returning...");
WOLF_WARN(
"Couldn't find a frame with valid and registered state blocks P and CaptureGnss with T, "
"returning...");
return false;
}
// only marginals of P and T
WOLF_DEBUG("SolverCeres::computeCovariances: storing pairs of frame ", frame->id(), " with state blocks: "
"\nP @", frame->getStateBlock('P'),
"\nT @", sb_gnss_T);
state_block_pairs.emplace_back(frame->getStateBlock('P'),
frame->getStateBlock('P'));
WOLF_DEBUG("Storing pairs of frame ",
frame->id(),
" with state blocks: "
"\nP @",
frame->getStateBlock('P'),
"\nT @",
sb_gnss_T);
state_block_pairs.emplace_back(frame->getStateBlock('P'), frame->getStateBlock('P'));
double_pairs.emplace_back(getAssociatedMemBlockPtr(frame->getStateBlock('P')),
getAssociatedMemBlockPtr(frame->getStateBlock('P')));
state_block_pairs.emplace_back(sb_gnss_T, sb_gnss_T);
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T),
getAssociatedMemBlockPtr(sb_gnss_T));
double_pairs.emplace_back(getAssociatedMemBlockPtr(sb_gnss_T), getAssociatedMemBlockPtr(sb_gnss_T));
break;
}
default:
throw std::runtime_error("SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
throw std::runtime_error(
"SolverCeres::computeCovariances: Unknown CovarianceBlocksToBeComputed enum value");
}
//std::cout << "pairs... " << double_pairs.size() << std::endl;
// std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
......@@ -416,7 +454,8 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(
state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize());
covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got switch: " << std::endl << cov << std::endl;
......@@ -424,13 +463,13 @@ bool SolverCeres::computeCovariancesDerived(const CovarianceBlocksToBeComputed _
return true;
}
WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
WOLF_WARN("Couldn't compute covariances!");
return false;
}
bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list)
{
//std::cout << "SolverCeres: computing covariances..." << std::endl;
// std::cout << "SolverCeres: computing covariances..." << std::endl;
// update problem
update();
......@@ -442,25 +481,26 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st
// double loop all against all (without repetitions)
for (auto st_it1 = st_list.begin(); st_it1 != st_list.end(); st_it1++)
{
if (*st_it1 == nullptr){
if (*st_it1 == nullptr)
{
continue;
}
assert(isStateBlockRegisteredDerived(*st_it1));
for (auto st_it2 = st_it1; st_it2 != st_list.end(); st_it2++)
{
if (*st_it2 == nullptr){
if (*st_it2 == nullptr)
{
continue;
}
assert(isStateBlockRegisteredDerived(*st_it2));
state_block_pairs.emplace_back(*st_it1, *st_it2);
double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)),
getAssociatedMemBlockPtr((*st_it2)));
double_pairs.emplace_back(getAssociatedMemBlockPtr((*st_it1)), getAssociatedMemBlockPtr((*st_it2)));
}
}
//std::cout << "pairs... " << double_pairs.size() << std::endl;
// std::cout << "pairs... " << double_pairs.size() << std::endl;
// COMPUTE DESIRED COVARIANCES
if (covariance_->Compute(double_pairs, ceres_problem_.get()))
......@@ -471,7 +511,8 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st
// STORE DESIRED COVARIANCES
for (unsigned int i = 0; i < double_pairs.size(); i++)
{
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(state_block_pairs[i].first->getLocalSize(),state_block_pairs[i].second->getLocalSize());
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> cov(
state_block_pairs[i].first->getLocalSize(), state_block_pairs[i].second->getLocalSize());
covariance_->GetCovarianceBlockInTangentSpace(double_pairs[i].first, double_pairs[i].second, cov.data());
wolf_problem_->addCovarianceBlock(state_block_pairs[i].first, state_block_pairs[i].second, cov);
// std::cout << "covariance got from st_list: " << std::endl << cov << std::endl;
......@@ -479,7 +520,7 @@ bool SolverCeres::computeCovariancesDerived(const std::vector<StateBlockPtr>& st
return true;
}
WOLF_WARN("SolverCeres::computeCovariances: Couldn't compute covariances!");
WOLF_WARN("Couldn't compute covariances!");
return false;
}
......@@ -487,7 +528,7 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
{
assert(!isFactorRegisteredDerived(fac_ptr) && "adding a factor that is already in the fac_2_costfunction_ map");
auto cost_func_ptr = createCostFunction(fac_ptr);
auto cost_func_ptr = createCostFunction(fac_ptr);
fac_2_costfunction_[fac_ptr] = cost_func_ptr;
std::vector<double*> res_block_mem;
......@@ -495,32 +536,37 @@ void SolverCeres::addFactorDerived(const FactorBasePtr& fac_ptr)
for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{
assert(isStateBlockRegisteredDerived(st) && "adding a factor that involves a floating or not registered sb");
res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
res_block_mem.emplace_back(getAssociatedMemBlockPtr(st));
}
auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
//std::cout << "Added residual block corresponding to constraint " << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() << std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
// std::cout << "Added residual block corresponding to constraint " <<
// std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->getType() <<
// std::static_pointer_cast<CostFunctionWrapper>(cost_func_ptr)->getConstraintPtr()->id() <<std::endl;
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
"ceres residuals different from wrapper residuals");
assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() &&
"adding a factor that is already in the fac_2_residual_idx_ map");
fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
loss_func_ptr,
res_block_mem);
fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(), loss_func_ptr, res_block_mem);
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
"ceres residuals different from wrapper residuals");
}
void SolverCeres::removeFactorDerived(const FactorBasePtr& _fac_ptr)
{
assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() &&
"removing a factor that is not in the fac_2_residual map");
ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]);
fac_2_residual_idx_.erase(_fac_ptr);
fac_2_costfunction_.erase(_fac_ptr);
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() &&
"ceres residuals different from wrapper residuals");
}
void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
......@@ -533,25 +579,24 @@ void SolverCeres::addStateBlockDerived(const StateBlockPtr& state_ptr)
if (state_ptr->hasLocalParametrization())
{
auto p = state_blocks_local_param_.emplace(state_ptr,
std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
auto p = state_blocks_local_param_.emplace(
state_ptr, std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
local_parametrization_ptr = p.first->second.get();
}
ceres_problem_->AddParameterBlock(getAssociatedMemBlockPtr(state_ptr),
state_ptr->getSize(),
local_parametrization_ptr);
ceres_problem_->AddParameterBlock(
getAssociatedMemBlockPtr(state_ptr), state_ptr->getSize(), local_parametrization_ptr);
if (state_ptr->isFixed())
ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
if (state_ptr->isFixed()) ceres_problem_->SetParameterBlockConstant(getAssociatedMemBlockPtr(state_ptr));
updateStateBlockStatusDerived(state_ptr);
}
void SolverCeres::removeStateBlockDerived(const StateBlockPtr& state_ptr)
{
//std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr) << std::endl;
// std::cout << "SolverCeres::removeStateBlock " << state_ptr.get() << " - " << getAssociatedMemBlockPtr(state_ptr)
// << std::endl;
assert(state_ptr);
assert(isStateBlockRegisteredDerived(state_ptr) && "removing a state block that is not in ceres");
......@@ -592,8 +637,7 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt
}
// Remove all involved factors (it does not remove any parameter block)
for (auto fac : involved_factors)
removeFactorDerived(fac);
for (auto fac : involved_factors) removeFactorDerived(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
removeStateBlockDerived(state_ptr);
......@@ -602,38 +646,41 @@ void SolverCeres::updateStateBlockLocalParametrizationDerived(const StateBlockPt
addStateBlockDerived(state_ptr);
// Add all involved factors
for (auto fac : involved_factors)
addFactorDerived(fac);
for (auto fac : involved_factors) addFactorDerived(fac);
}
bool SolverCeres::hasConverged()
bool SolverCeres::converged() const
{
return summary_.termination_type == ceres::CONVERGENCE;
}
bool SolverCeres::wasStopped()
bool SolverCeres::wasStopped() const
{
return summary_.termination_type == ceres::USER_FAILURE or summary_.termination_type == ceres::USER_SUCCESS;
}
unsigned int SolverCeres::iterations()
bool SolverCeres::failed() const
{
if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1)
return 0;
return summary_.termination_type == ceres::USER_FAILURE or summary_.termination_type == ceres::FAILURE;
}
unsigned int SolverCeres::iterations() const
{
if (summary_.num_successful_steps + summary_.num_unsuccessful_steps < 1) return 0;
return summary_.num_successful_steps + summary_.num_unsuccessful_steps;
}
double SolverCeres::initialCost()
double SolverCeres::initialCost() const
{
return double(summary_.initial_cost);
}
double SolverCeres::finalCost()
double SolverCeres::finalCost() const
{
return double(summary_.final_cost);
}
double SolverCeres::totalTime()
double SolverCeres::totalTime() const
{
return double(summary_.total_time_in_seconds);
}
......@@ -651,7 +698,7 @@ ceres::CostFunctionPtr SolverCeres::createCostFunction(const FactorBasePtr& _fac
return createNumericDiffCostFunction(_fac_ptr);
else
throw std::invalid_argument( "Wrong Jacobian Method!" );
throw std::invalid_argument("Wrong Jacobian Method!");
}
bool SolverCeres::checkDerived(std::string prefix) const
......@@ -663,22 +710,36 @@ bool SolverCeres::checkDerived(std::string prefix) const
ceres_problem_->NumResidualBlocks() != fac_2_costfunction_.size() or
ceres_problem_->NumResidualBlocks() != fac_2_residual_idx_.size())
{
WOLF_ERROR("SolverCeres::check: number of residuals mismatch - in ", prefix, ":\n\tceres_problem_->NumResidualBlocks(): ", ceres_problem_->NumResidualBlocks(), "\n\tfactors_.size(): ", factors_.size(), "\n\tfac_2_costfunction_.size(): ", fac_2_costfunction_.size(), "\n\tfac_2_residual_idx_.size(): ", fac_2_residual_idx_.size());
WOLF_ERROR("Number of residuals mismatch - in ",
prefix,
":\n\tceres_problem_->NumResidualBlocks(): ",
ceres_problem_->NumResidualBlocks(),
"\n\tfactors_.size(): ",
factors_.size(),
"\n\tfac_2_costfunction_.size(): ",
fac_2_costfunction_.size(),
"\n\tfac_2_residual_idx_.size(): ",
fac_2_residual_idx_.size());
ok = false;
}
if (ceres_problem_->NumParameterBlocks() != state_blocks_.size())
{
WOLF_ERROR("SolverCeres::check: number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ", ceres_problem_->NumParameterBlocks(), " != state_blocks_.size(): ", state_blocks_.size(), ") - in ", prefix);
WOLF_ERROR("Number of parameters mismatch ((ceres_problem_->NumParameterBlocks(): ",
ceres_problem_->NumParameterBlocks(),
" != state_blocks_.size(): ",
state_blocks_.size(),
") - in ",
prefix);
ok = false;
}
// Check parameter blocks
//for (auto&& state_block_pair : state_blocks_)
// for (auto&& state_block_pair : state_blocks_)
for (const auto& state_block_pair : state_blocks_)
{
if (!ceres_problem_->HasParameterBlock(state_block_pair.second.data()))
{
WOLF_ERROR("SolverCeres::check: any state block is missing in ceres problem - in ", prefix);
WOLF_ERROR("Any state block is missing in ceres problem - in ", prefix);
ok = false;
}
}
......@@ -689,27 +750,31 @@ bool SolverCeres::checkDerived(std::string prefix) const
// SolverManager::factors_
if (factors_.count(fac_res_pair.first) == 0)
{
WOLF_ERROR("SolverCeres::check: factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
WOLF_ERROR("Factor ", fac_res_pair.first->id(), " (in fac_2_residual_idx_) not in factors_ - in ", prefix);
ok = false;
}
// costfunction - residual
if (fac_2_costfunction_.count(fac_res_pair.first) == 0)
{
WOLF_ERROR("SolverCeres::check: any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
WOLF_ERROR("Any factor in fac_2_residual_idx_ is not in fac_2_costfunction_ - in ", prefix);
ok = false;
}
//if (fac_2_costfunction_[fac_res_pair.first].get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
if (fac_2_costfunction_.at(fac_res_pair.first).get() != ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
// if (fac_2_costfunction_[fac_res_pair.first].get() !=
// ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
if (fac_2_costfunction_.at(fac_res_pair.first).get() !=
ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
{
WOLF_ERROR("SolverCeres::check: fac_2_costfunction_ and ceres mismatch - in ", prefix);
WOLF_ERROR("fac_2_costfunction_ and ceres mismatch - in ", prefix);
ok = false;
}
// factor - residual
if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor())
if (fac_res_pair.first != static_cast<const CostFunctionWrapper*>(
ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))
->getFactor())
{
WOLF_ERROR("SolverCeres::check: fac_2_residual_idx_ and ceres mismatch - in ", prefix);
WOLF_ERROR("fac_2_residual_idx_ and ceres mismatch - in ", prefix);
ok = false;
}
......@@ -718,7 +783,12 @@ bool SolverCeres::checkDerived(std::string prefix) const
ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, &param_blocks);
if (param_blocks.size() != fac_res_pair.first->getStateBlockPtrVector().size())
{
WOLF_ERROR("SolverCeres::check: number parameter blocks in ceres residual", param_blocks.size(), " different from number of state blocks in factor ", fac_res_pair.first->getStateBlockPtrVector().size(), " - in ", prefix);
WOLF_ERROR("number parameter blocks in ceres residual",
param_blocks.size(),
" different from number of state blocks in factor ",
fac_res_pair.first->getStateBlockPtrVector().size(),
" - in ",
prefix);
ok = false;
}
else
......@@ -728,7 +798,7 @@ bool SolverCeres::checkDerived(std::string prefix) const
{
if (getAssociatedMemBlockPtr(st) != param_blocks[i])
{
WOLF_ERROR("SolverCeres::check: parameter", i, " mismatch - in ", prefix);
WOLF_ERROR("parameter", i, " mismatch - in ", prefix);
ok = false;
}
i++;
......@@ -738,11 +808,14 @@ bool SolverCeres::checkDerived(std::string prefix) const
return ok;
}
void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
void insertSparseBlock(const Eigen::MatrixXd& ins,
Eigen::SparseMatrixd& original,
const unsigned int& row,
const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
original.insert(row + ins_row, col + ins_col) = ins(ins_row, ins_col);
original.makeCompressed();
}
......@@ -754,17 +827,19 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
// fac_2_residual_idx_
// fac_2_costfunction_
// state_blocks_local_param_
int ix_row = 0; // index of the factor/measurement in the
for (auto fac_res_pair: fac_2_residual_idx_){
int ix_row = 0; // index of the factor/measurement in the
for (auto fac_res_pair : fac_2_residual_idx_)
{
FactorBasePtr fac_ptr = fac_res_pair.first;
ix_row += fac_ptr->getSize();
// retrieve all stateblocks data related to this factor
std::vector<const double*> fac_states_ptr;
for (auto sb : fac_res_pair.first->getStateBlockPtrVector()){
for (auto sb : fac_res_pair.first->getStateBlockPtrVector())
{
fac_states_ptr.push_back(sb->getStateData());
}
// retrieve residual value, not used afterwards in this case since we just care about jacobians
Eigen::VectorXd residual(fac_ptr->getSize());
// retrieve jacobian in group size, not local size
......@@ -782,58 +857,59 @@ const Eigen::SparseMatrixd SolverCeres::computeHessian() const
assert((A.cols() >= sb->getLocalSize() + jacobians[i].cols() - 1) && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
if (sb->hasLocalParametrization()){
// if the state block has a local parameterization, we need to right multiply by the manifold element / tangent element jacobian
Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
if (sb->hasLocalParametrization())
{
// if the state block has a local parameterization, we need to right multiply by the manifold
// element / tangent element jacobian
Eigen::MatrixXd J_manif_tang(sb->getSize(), sb->getLocalSize());
Eigen::Map<Eigen::MatrixRowXd> J_manif_tang_map(
J_manif_tang.data(), sb->getSize(), sb->getLocalSize());
Eigen::Map<const Eigen::VectorXd> sb_state_map(sb->getStateData(), sb->getSize());
sb->getLocalParametrization()->computeJacobian(sb_state_map, J_manif_tang_map);
insertSparseBlock(jacobians[i] * J_manif_tang, A_block_row, 0, sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
insertSparseBlock(jacobians[i] * J_manif_tang,
A_block_row,
0,
sb->getLocalSize()); // (to_insert, matrix_to_fill, row, col)
}
else {
else
{
insertSparseBlock(jacobians[i], A_block_row, 0, sb->getLocalSize());
}
}
}
// fill the weighted jacobian matrix block row
A.block(ix_row, 0, fac_ptr->getSize(), A.cols());
}
// compute the hessian matrix from the weighted jacobian
// compute the hessian matrix from the weighted jacobian
H = A.transpose() * A;
return H;
}
bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param)
bool SolverCeres::hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) const
{
return state_blocks_local_param_.count(st) == 1
&& state_blocks_local_param_.at(st)->getLocalParametrization() == local_param
&& ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st))
== state_blocks_local_param_.at(st).get();
return state_blocks_local_param_.count(st) == 1 &&
state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
}
void SolverCeres::printProfilingDerived(std::ostream& _stream) const
{
auto n_solve = profiling_solve_.getNCallbacks();
_stream << "Iterations:"
<< "\n\tUser-defined limit: " << params_ceres_->solver_options.max_num_iterations
<< "\n\tAverage iterations: " << n_iter_acc_ / n_solve_
<< "\n\tMax. iterations: " << n_iter_max_
<< "\nTermination:"
<< "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve_ << " %"
<< "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve_ << " %"
<< "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve_ << " %"
<< std::endl;
<< "\n\tUser-defined limit: " << solver_options_.max_num_iterations
<< "\n\tAverage iterations: " << n_iter_acc_ / n_solve
<< "\n\tMax. iterations: " << n_iter_max_ << "\nTermination:"
<< "\n\tConvergence: " << 1e2 * n_convergence_ / n_solve << " %"
<< "\n\tInterrupted by problem change: " << 1e2 * n_interrupted_ / n_solve << " %"
<< "\n\tMax. iterations reached: " << 1e2 * n_no_convergence_ / n_solve << " %" << std::endl;
}
// register in the factory
WOLF_REGISTER_SOLVER(SolverCeres)
} // namespace wolf
#include "core/solver/factory_solver.h"
namespace wolf {
WOLF_REGISTER_SOLVER(SolverCeres)
} // namespace wolf
} // namespace wolf
src/common/node_base.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/common/node_base.h"
namespace wolf {
//init static node counter
namespace wolf
{
// init static node counter
unsigned int NodeBase::node_id_count_ = 0;
} // namespace wolf
} // namespace wolf
src/common/node_state_blocks.cpp 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/common/node_state_blocks.h"
#include "core/utils/string_utils.h"
#include "core/state_block/factory_state_block.h"
#include "core/math/covariance.h"
#include "core/factor/factor_block_absolute.h"
#include "core/factor/factor_quaternion_absolute.h"
#include "core/factor/factor_homogeneous_3d_absolute.h"
using namespace Eigen;
namespace wolf
{
NodeStateBlocks::NodeStateBlocks(const std::string& _category,
const std::string& _type,
const TypeComposite& _state_types,
const VectorComposite& _state_vectors,
const PriorComposite& _state_priors)
: NodeBase(_category, _type), state_priors_(_state_priors), state_types_(_state_types)
{
if (not _state_types.has(_state_vectors.getKeys()))
{
throw std::runtime_error("NodeStateBlocks: In constructor of type " + _type +
", provided any '_state_vectors' (" + _state_vectors.getKeys() +
") that is not in '_state_types' (" + _state_types.getKeys() + ").");
}
checkTypeComposite(_state_types);
for (auto vec_pair : _state_vectors)
emplaceStateBlock(vec_pair.first,
_state_types.at(vec_pair.first),
vec_pair.second,
_state_priors.count(vec_pair.first) ? _state_priors.at(vec_pair.first).isFixed() : false);
}
void NodeStateBlocks::remove(bool viral_remove_parent_without_children)
{
WOLF_ERROR_COND(isSensor(), "Shouldn't be called in Sensors!");
// remove state blocks
for (const char key : getKeys())
{
auto sbp = getStateBlock(key);
if (sbp != nullptr)
{
// notification to problem inside the function
removeStateBlock(key);
}
}
// remove factors
while (not factored_by_set_.empty())
{
// notification to problem inside the function
(*factored_by_set_.begin())->remove(viral_remove_parent_without_children);
}
factor_sensory_set_.clear();
factor_prior_map_.clear();
}
StateBlockPtr NodeStateBlocks::emplaceStateBlock(const char _key,
const std::string& _type,
const VectorXd& _vector,
const bool& _fixed)
{
// check key available
if (state_blocks_.count(_key))
{
throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlock: key ") + _key +
" already in the node.");
}
// check correct type
checkTypeComposite({{_key, _type}});
if (state_types_.count(_key))
{
if (state_types_.at(_key) != _type)
throw std::runtime_error(
std::string("NodeStateBlocks::emplaceStateBlock: Wrong input type: " + _type + " for key ") + _key +
". It should be " + state_types_.at(_key));
}
else
state_types_[_key] = _type;
// create state block
auto sb = FactoryStateBlock::create(_type, _vector, _fixed);
// if local parameterization, check is valid (normalized for quaternions)
if (sb->hasLocalParametrization() and not sb->getLocalParametrization()->isValid(_vector, 1e-4))
throw std::runtime_error(
std::string("NodeStateBlocks::emplaceStateBlock: State values provided for key '") + _key + "' of type '" +
sb->getType() +
"' are not valid according the local parameterization (tolerance: 1e-4): " + std::to_string(_vector));
// store state block
state_blocks_.emplace(_key, sb);
// register to problem (if linked)
if (getProblem()) getProblem()->notifyStateBlock(sb, ADD);
return sb;
}
StateBlockPtr NodeStateBlocks::emplaceStateBlockZero(const char _key, const std::string& _type, const bool& _fixed)
{
// check key available
if (state_blocks_.count(_key))
{
throw std::runtime_error(std::string("NodeStateBlocks::emplaceStateBlockZero: key ") + _key +
" already in the node.");
}
// check correct type
checkTypeComposite({{_key, _type}});
if (state_types_.count(_key))
{
if (state_types_.at(_key) != _type)
throw std::runtime_error(
std::string("NodeStateBlocks::emplaceStateBlockZero: Wrong input type: " + _type + " for key ") +
_key + ". It should be " + state_types_.at(_key));
}
else
state_types_[_key] = _type;
// create state block
auto sb = FactoryStateBlockIdentity::create(_type, _fixed);
// store state block
state_blocks_.emplace(_key, sb);
// register to problem (if linked)
if (getProblem()) getProblem()->notifyStateBlock(sb, ADD);
return sb;
}
void NodeStateBlocks::removeStateBlock(const char& _key)
{
StateBlockPtr _sb = getStateBlock(_key);
// check key exists
if (not _sb)
{
WOLF_WARN("Key '", _key, "' is not in the node. Skipping...");
return;
}
// Do not modify 'state_types_': if the state is added again, the type needs to be checked.
// Remove from 'state_priors_' just in case priors are not emplaced yet
if (state_priors_.count(_key))
{
state_priors_.erase(_key);
}
// Remove prior factors (automatically notified)
if (factor_prior_map_.count(_key))
{
factor_prior_map_.at(_key)->remove();
}
// notify state removal to problem (if linked)
if (getProblem()) getProblem()->notifyStateBlock(_sb, REMOVE);
// And finally remove state block
state_blocks_.erase(_key);
}
void NodeStateBlocks::emplaceFactorStateBlock(const char& _key,
const VectorXd& _x,
const MatrixXd& _cov,
unsigned int _start_idx)
{
assert(isCovariance(_cov) and
"NodeStateBlocks::emplaceFactorStateBlock: provided _cov is not a covariance matrix");
StateBlockPtr _sb = getStateBlock(_key);
// CHECKS
// missing key
if (not _sb) throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: missing key " + _key);
// Overparametrized states
if (_sb->getSize() != _sb->getLocalSize())
{
// not segment of state not allowed
if (_start_idx != 0)
throw std::runtime_error(
"NodeStateBlocks::emplaceFactorStateBlock: Prior factor for a segment of a state with local size "
"different from size not allowed");
// meas size
if (_x.size() != _sb->getSize())
throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad measurement size");
// cov size
if (_cov.cols() != _sb->getLocalSize())
throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad covariance size");
}
else
{
// cov and state size
if (_cov.cols() != _x.size())
throw std::runtime_error(
"NodeStateBlocks::emplaceFactorStateBlock: Inconsistend measurement and covariance sizes");
// meas size
if (_x.size() + _start_idx > _sb->getSize())
throw std::runtime_error("NodeStateBlocks::emplaceFactorStateBlock: Bad measurement size");
}
// existing prior
if (factor_prior_map_.count(_key))
throw std::runtime_error(
std::string("NodeStateBlocks::emplaceFactorStateBlock: There already is a factor prior for this key ") +
_key + ", factor " + std::to_string(factor_prior_map_.at(_key)->id()));
// EMPLACE FACTOR
if (std::dynamic_pointer_cast<StateQuaternion>(_sb))
{
factor_prior_map_[_key] = FactorBase::emplace<FactorQuaternionAbsolute>(
nullptr, _x, computeSqrtUpper(_cov.inverse()), shared_from_this(), _key, nullptr, false);
}
else if (std::dynamic_pointer_cast<StateHomogeneous3d>(_sb))
{
factor_prior_map_[_key] = FactorBase::emplace<FactorHomogeneous3dAbsolute>(
nullptr, _x, computeSqrtUpper(_cov.inverse()), shared_from_this(), _key, nullptr, false);
}
else
{
factor_prior_map_[_key] = FactorBase::emplace<FactorBlockAbsolute>(nullptr,
_x,
computeSqrtUpper(_cov.inverse()),
shared_from_this(),
_key,
_start_idx,
_x.size(),
nullptr,
false);
}
}
void NodeStateBlocks::emplacePriors()
{
for (auto prior_pair : state_priors_)
{
auto key = prior_pair.first;
auto prior = prior_pair.second;
// ignore priors to missing keys
if (not state_blocks_.count(key))
{
WOLF_WARN("Key ", key, " not in the node. Ignoring prior for this key.");
continue;
}
// fix (if specified)
if (prior.isFixed()) state_blocks_.at(key)->fix();
// emplace factor prior (if specified)
WOLF_DEBUG_COND(
prior.isFactor(), "Emplacing factor in '", key, "' with factor_std: ", prior.getFactorStd().transpose());
if (prior.isFactor())
emplaceFactorStateBlock(
key, state_blocks_.at(key)->getState(), prior.getFactorStd().cwiseAbs2().asDiagonal());
}
state_priors_.clear();
}
YAML::Node NodeStateBlocks::toYaml() const
{
YAML::Node node;
node["type"] = NodeBase::node_type_;
for (auto&& state_pair : state_blocks_)
{
auto key_str = std::string(1, state_pair.first);
node["states"][key_str]["type"] = state_pair.second->getType();
node["states"][key_str]["value"] = state_pair.second->getState();
if (factor_prior_map_.count(state_pair.first) != 0)
{
node["states"][key_str]["prior"]["mode"] = "factor";
MatrixXd cov_sqrt =
factor_prior_map_.at(state_pair.first)->getMeasurementSquareRootInformationUpper().inverse();
node["states"][key_str]["prior"]["factor_std"] =
VectorXd((cov_sqrt * cov_sqrt.transpose()).diagonal().cwiseSqrt());
WOLF_DEBUG("info_sqrt: \n",
factor_prior_map_.at(state_pair.first)->getMeasurementSquareRootInformationUpper());
WOLF_DEBUG("cov_sqrt: \n", cov_sqrt);
WOLF_DEBUG("cov: \n", cov_sqrt * cov_sqrt.transpose());
WOLF_DEBUG("factor_std: \n", VectorXd((cov_sqrt * cov_sqrt.transpose()).diagonal()).transpose());
}
else
node["states"][key_str]["prior"]["mode"] = state_pair.second->isFixed() ? "fix" : "initial_guess";
}
return node;
}
void NodeStateBlocks::setProblem(ProblemPtr _problem)
{
if (getProblem() or not _problem) return;
NodeBase::setProblem(_problem);
// register state blocks
for (auto pair_key_sbp : getStateBlockMap())
if (pair_key_sbp.second != nullptr) _problem->notifyStateBlock(pair_key_sbp.second, ADD);
// register prior factors
for (auto pair_key_prior : getPriorFactorMap())
if (pair_key_prior.second != nullptr) _problem->notifyFactor(pair_key_prior.second, ADD);
}
bool NodeStateBlocks::getCovariance(const StateKeys& _keys, MatrixXd& _cov) const
{
if (not this->has(_keys))
{
WOLF_WARN("Does not have the keys ", _keys, ". Available keys: ", getKeys());
return false;
}
bool success(true);
// resizing
SizeEigen sz = getLocalSize();
_cov.resize(sz, sz);
// filling covariance
int i = 0, j = 0;
for (auto key_i : _keys)
{
auto sb_i = getStateBlock(key_i);
j = 0;
for (auto key_j : _keys)
{
auto sb_j = getStateBlock(key_j);
success = success && getProblem()->getCovarianceBlock(sb_i, sb_j, _cov, i, j);
j += sb_j->getLocalSize();
}
i += sb_i->getLocalSize();
}
return success;
}
FactorBaseConstPtrList NodeStateBlocks::getFactorsOf(ProcessorBaseConstPtr _processor_ptr,
const std::string& type) const
{
FactorBaseConstPtrList list;
for (auto factor_ptr : getFactoredBySet())
if (factor_ptr->getProcessor() == _processor_ptr and (type.empty() or factor_ptr->getType() == type))
list.push_back(factor_ptr);
return list;
}
FactorBasePtrList NodeStateBlocks::getFactorsOf(ProcessorBasePtr _processor_ptr, const std::string& type)
{
FactorBasePtrList list;
for (auto factor_ptr : getFactoredBySet())
if (factor_ptr->getProcessor() == _processor_ptr and (type.empty() or factor_ptr->getType() == type))
list.push_back(factor_ptr);
return list;
}
void NodeStateBlocks::setState(const VectorComposite& _state, const bool _notify)
{
for (const auto& pair_key_vec : _state)
{
const auto& key = pair_key_vec.first;
const auto& vec = pair_key_vec.second;
const auto& sb = getStateBlock(key);
if (!sb) WOLF_ERROR("Stateblock key ", key, " not in the keys");
assert(sb and "Stateblock key not in the keys");
sb->setState(vec, _notify);
}
}
void NodeStateBlocks::setState(const VectorXd& _state, const StateKeys& _keys, const bool _notify)
{
int size = getSize(_keys);
assert(_state.size() == size and "In FrameBase::setState wrong state size");
unsigned int index = 0;
for (const char key : _keys)
{
const auto& sb = getStateBlock(key);
assert(sb and "Stateblock key not in the keys");
sb->setState(_state.segment(index, sb->getSize()),
_notify); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
void NodeStateBlocks::setState(const VectorXd& _state,
const StateKeys& _keys,
const std::list<SizeEigen>& _sizes,
const bool _notify)
{
SizeEigen index = 0;
auto size_it = _sizes.begin();
for (const auto& key : _keys)
{
const auto& sb = getStateBlock(key);
assert(sb and "Stateblock key not in the keys");
assert(*size_it == sb->getSize() and "State block size mismatch");
sb->setState(_state.segment(index, *size_it), _notify);
index += *size_it;
size_it++;
}
}
void NodeStateBlocks::setState(const StateKeys& _keys, const std::list<VectorXd>& _vectors, const bool _notify)
{
auto vec_it = _vectors.begin();
for (const auto key : _keys)
{
const auto& sb = getStateBlock(key);
assert(sb and "Stateblock key not in the keys");
assert(vec_it->size() == sb->getSize() and "State block size mismatch");
sb->setState(*vec_it, _notify);
vec_it++;
}
}
VectorXd NodeStateBlocks::getStateVector(const StateKeys& _keys) const
{
// _keys can be either stateblock keys of the node or a subset of this keys
VectorXd state(getSize(_keys));
unsigned int index = 0;
for (const char key : _keys)
{
const auto& sb = getStateBlock(key);
assert(sb != nullptr and "Requested StateBlock key not in the keys");
state.segment(index, sb->getSize()) = sb->getState();
index += sb->getSize();
}
return state;
}
VectorComposite NodeStateBlocks::getState(const StateKeys& _keys) const
{
const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
VectorComposite state;
for (const auto key : keys)
{
auto state_it = state_blocks_.find(key);
if (state_it != state_blocks_.end()) state.emplace(key, state_it->second->getState());
}
return state;
}
unsigned int NodeStateBlocks::getSize(const StateKeys& _keys) const
{
const StateKeys& keys = (_keys == "" ? getKeys() : _keys);
unsigned int size = 0;
for (const char key : keys)
{
const auto& sb = getStateBlock(key);
if (!sb)
{
WOLF_ERROR("Stateblock key ", key, " not in the keys");
}
size += sb->getSize();
}
return size;
}
bool NodeStateBlocks::isFixed() const
{
return std::all_of(state_blocks_.cbegin(),
state_blocks_.cend(),
[](const std::pair<char, StateBlockPtr>& sb_pair) { return sb_pair.second->isFixed(); });
}
bool NodeStateBlocks::hasStateBlock(const StateBlockConstPtr& _sb) const
{
const auto& it = std::find_if(state_blocks_.cbegin(),
state_blocks_.cend(),
[_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
return it != state_blocks_.cend();
}
bool NodeStateBlocks::stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const
{
const auto& it = std::find_if(state_blocks_.cbegin(),
state_blocks_.cend(),
[_sb](const std::pair<char, StateBlockPtr>& pair) { return pair.second == _sb; });
if (it != state_blocks_.cend())
{
_key = it->first;
return true;
}
else
{
_key = '0'; // '0' = not found
return false;
}
}
unsigned int NodeStateBlocks::getLocalSize(const StateKeys& _keys) const
{
StateKeys keys;
if (_keys == "")
keys = getKeys();
else
keys = _keys;
unsigned int size = 0;
for (const char key : keys)
{
const auto& sb = getStateBlock(key);
if (!sb)
{
WOLF_ERROR("Stateblock key ", key, " not in the keys");
}
size += sb->getLocalSize();
}
return size;
}
FactorBasePtr NodeStateBlocks::addFactor(FactorBasePtr _fac_ptr)
{
factored_by_set_.insert(_fac_ptr);
if (_fac_ptr->getFeature()) factor_sensory_set_.insert(_fac_ptr);
return _fac_ptr;
}
void NodeStateBlocks::removeFactor(FactorBasePtr _fac_ptr)
{
factor_sensory_set_.erase(_fac_ptr);
factored_by_set_.erase(_fac_ptr);
const auto& it = std::find_if(
factor_prior_map_.begin(), factor_prior_map_.end(), [_fac_ptr](const std::pair<char, FactorBasePtr>& pair) {
return pair.second == _fac_ptr;
});
if (it != factor_prior_map_.end()) factor_prior_map_.erase(it);
}
void NodeStateBlocks::perturb(double amplitude)
{
for (const auto& pair_key_sb : state_blocks_)
{
auto& sb = pair_key_sb.second;
if (!sb->isFixed()) sb->perturb(amplitude);
}
}
void NodeStateBlocks::transform(const VectorComposite& _transformation)
{
for (auto& pair_key_sb : state_blocks_) pair_key_sb.second->transform(_transformation);
}
void NodeStateBlocks::printState(bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
if (_metric && _state_blocks)
{
for (auto key : getKeys())
{
auto sb = getStateBlock(key);
if (sb)
{
_stream << _tabs << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
<< std::setprecision(3) << sb->getState().transpose() << " )";
auto fac = getPriorFactor(key);
if (_factored_by and fac)
{
_stream << " <-- Fac" << fac->id() << ": mean = ( " << fac->getMeasurement().transpose() << " )";
_stream << " , std = ( "
<< fac->getMeasurementSquareRootInformationUpper().diagonal().cwiseInverse().transpose()
<< " )";
}
_stream << " @ " << sb << std::endl;
}
}
}
else if (_metric)
{
_stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3)
<< getStateVector(getKeys()).transpose() << " )" << std::endl;
}
else if (_state_blocks)
{
for (auto key : getKeys())
{
auto sb = getStateBlock(key);
if (sb)
{
_stream << " " << key << " [" << (sb->isFixed() ? "Fix" : "Est") << "] ";
auto fac = getPriorFactor(key);
if (_factored_by and fac)
{
_stream << " <-- Fac" << fac->id();
}
_stream << " @ " << sb << std::endl;
}
}
_stream << std::endl;
}
}
CheckLog NodeStateBlocks::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
std::stringstream inconsistency_explanation;
for (auto pair : getStateBlockMap())
{
auto sb = pair.second;
// check for valid state block
inconsistency_explanation << getCategory() << " " << id() << " @ " << shared_from_this().get()
<< " has State block pointer " << sb.get() << " = 0 \n";
log.assertTrue((sb.get() != 0), inconsistency_explanation);
if (_verbose)
{
_stream << _tabs << " " << pair.first << " sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp) _stream << " (lp @ " << lp.get() << ")";
}
_stream << std::endl;
}
}
// check factored_by
for (auto fac : getFactoredBySet())
{
if (_verbose)
{
_stream << _tabs << " "
<< "<- Fac" << fac->id() << " -> ";
for (auto Nw : fac->getNodesFactored()) _stream << " " << Nw.lock()->getCategory() << Nw.lock()->id();
_stream << std::endl;
}
for (auto sb : fac->getStateBlockPtrVector())
{
if (_verbose)
{
_stream << _tabs << " "
<< "sb @ " << sb.get();
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp) _stream << " (lp @ " << lp.get() << ")";
}
_stream << std::endl;
}
}
// check factored_by pointer to this node
inconsistency_explanation << "Fac" << fac->id() << " factored by of " << getCategory() << id() << " @ "
<< shared_from_this().get() << " not found among factored nodes of the factor\n";
log.assertTrue(fac->hasNode(shared_from_this()), inconsistency_explanation);
// check factored_by pointer in factor_sensory_set_ or factor_prior_map_
inconsistency_explanation << "Fac" << fac->id() << " factored by of " << getCategory() << id() << " @ "
<< shared_from_this().get()
<< " not found in factor_sensory_set_ or factor_prior_map_\n";
log.assertTrue(
getSensoryFactorSet().count(fac) or std::find_if(factor_prior_map_.cbegin(),
factor_prior_map_.cend(),
[fac](const std::pair<char, FactorBasePtr>& pair) {
return pair.second == fac;
}) != factor_prior_map_.cend(),
inconsistency_explanation);
}
// check factor_sensory does not have any factor not in factored_by
for (auto fac_sen : getSensoryFactorSet())
{
inconsistency_explanation << "Fac" << fac_sen->id() << " sensory factor in " << getCategory() << id() << " @ "
<< shared_from_this().get() << " not found in factored_by_set_\n";
log.assertTrue(getFactoredBySet().count(fac_sen), inconsistency_explanation);
}
// check factor_prior_map_ does not have any factor not in factored_by
for (auto fac_pri : getPriorFactorMap())
{
inconsistency_explanation << "Fac" << fac_pri.second->id() << " prior factor in " << getCategory() << id()
<< " @ " << shared_from_this().get() << " not found in factored_by_set_\n";
log.assertTrue(getFactoredBySet().count(fac_pri.second), inconsistency_explanation);
}
return log;
}
} // namespace wolf
src/common/profiling_unit.cpp 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/common/profiling_unit.h"
#include <cassert>
namespace wolf
{
ProfilingUnit::ProfilingUnit(double desired_period_sec)
: running_(false),
desired_period_us_(1e6 * desired_period_sec),
n_callbacks_(0),
average_period_us_(0),
acc_duration_us_(0),
max_duration_us_(0),
var_duration_us_(0)
{
}
void ProfilingUnit::startProfiling()
{
if (running_) throw std::runtime_error("ProfilingUnit::startProfiling: profiling already running");
n_callbacks_++;
running_ = true;
start_clock_ = std::chrono::high_resolution_clock::now();
}
void ProfilingUnit::stopProfiling()
{
if (not running_) return;
assert(n_callbacks_ > 0);
// compute duration first of all
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() -
start_clock_);
// average period
if (n_callbacks_ == 1)
first_clock_ = start_clock_;
else
{
auto duration_from_start = std::chrono::duration_cast<std::chrono::microseconds>(start_clock_ - first_clock_);
average_period_us_ = duration_from_start.count() / (n_callbacks_ - 1);
}
// recursive formula for variance: https://math.stackexchange.com/a/2746016
if (n_callbacks_ == 1)
var_duration_us_ = 0;
else // n_callbacks_ > 1
{
double n_callbacks_d = n_callbacks_;
auto prev_mean_duration = acc_duration_us_.count() / (n_callbacks_d - 1);
var_duration_us_ = ((n_callbacks_d - 1) / n_callbacks_d) *
(var_duration_us_ + std::pow(prev_mean_duration - duration.count(), 2) / n_callbacks_d);
}
// max and accumulated duration
max_duration_us_ = std::max(max_duration_us_, duration);
acc_duration_us_ += duration;
running_ = false;
}
std::string ProfilingUnit::printDuration(double _duration_us, int precision) const
{
std::stringstream stream;
if (_duration_us >= 3600 * 1e6)
{
auto hours = static_cast<int>(_duration_us / (3600 * 1e6));
auto minutes = static_cast<int>((_duration_us - hours * 3600 * 1e6) / (60 * 1e6));
auto seconds = (_duration_us - hours * 3600 * 1e6 - minutes * 60 * 1e6) * 1e-6;
stream << hours << " h " << minutes << " min " << std::fixed << std::setprecision(precision) << seconds
<< " s";
}
else if (_duration_us >= 60 * 1e6)
{
auto minutes = static_cast<int>(_duration_us / (60 * 1e6));
auto seconds = (_duration_us - minutes * 60 * 1e6) * 1e-6;
stream << minutes << " min " << std::fixed << std::setprecision(precision) << seconds << " s";
}
else if (_duration_us >= 1e6)
stream << std::fixed << std::setprecision(precision) << 1e-6 * _duration_us << " s";
else if (_duration_us >= 1e3)
stream << std::fixed << std::setprecision(precision) << 1e-3 * _duration_us << " ms";
else
stream << std::fixed << std::setprecision(precision) << _duration_us << " us";
return stream.str();
}
void ProfilingUnit::printProfiling(const std::string& _tabs, std::ostream& _stream) const
{
auto ref_period_us = desired_period_us_ > 0 ? desired_period_us_ : average_period_us_;
unsigned int average_percent = (ref_period_us > 0) ? std::round(100 * 1e6 * getAvgDuration() / ref_period_us) : 0;
unsigned int max_percent = (ref_period_us > 0) ? std::round(100 * max_duration_us_.count() / ref_period_us) : 0;
unsigned int stdev_percent = (ref_period_us > 0) ? std::round(100 * sqrt(var_duration_us_) / ref_period_us) : 0;
_stream << "\n"
<< _tabs << "instances: " << n_callbacks_ //
<< "\n"
<< _tabs << "total time: " << printDuration(acc_duration_us_.count()) //
<< "\n";
if (desired_period_us_ > 0)
_stream << _tabs << "desired period: " << printDuration(desired_period_us_) //
<< " (reference)\n";
_stream << _tabs << "avg period: " << printDuration(average_period_us_) //
<< (desired_period_us_ > 0 ? "\n" : " (reference)\n") << _tabs
<< "avg time: " << printDuration(1e6 * getAvgDuration()) //
<< " (" << average_percent << "%)"
<< "\n"
<< _tabs << "stdev time: " << printDuration(sqrt(var_duration_us_)) //
<< " (" << stdev_percent << "%)"
<< "\n"
<< _tabs << "max time: " << printDuration(max_duration_us_.count()) //
<< " (" << max_percent << "%)";
}
} // namespace wolf
src/common/time_stamp.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/common/time_stamp.h"
namespace wolf {
TimeStamp TimeStamp::Now ( )
namespace wolf
{
TimeStamp TimeStamp::Now()
{
TimeStamp t(0);
t.setToNow();
......@@ -33,46 +29,41 @@ TimeStamp TimeStamp::Now ( )
std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts)
{
if (!_ts.ok())
os << "TimeStamp is invalid! ";
os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right <<_ts.getNanoSeconds(); // write obj to stream
if (!_ts.ok()) os << "TimeStamp is invalid! ";
os << _ts.getSeconds() << "." << std::setfill('0') << std::setw(9) << std::right
<< _ts.getNanoSeconds(); // write obj to stream
os << std::setfill(' ');
return os;
}
TimeStamp::TimeStamp() :
//time_stamp_(0)
time_stamp_nano_(0)
,
is_valid_(false)
TimeStamp::TimeStamp()
: // time_stamp_(0)
time_stamp_nano_(0),
is_valid_(false)
{
// setToNow();
//
}
TimeStamp::TimeStamp(const TimeStamp& _ts) :
time_stamp_nano_(_ts.time_stamp_nano_),
is_valid_(_ts.is_valid_)
TimeStamp::TimeStamp(const TimeStamp& _ts) : time_stamp_nano_(_ts.time_stamp_nano_), is_valid_(_ts.is_valid_)
{
//
}
TimeStamp::TimeStamp(const double& _ts) :
time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts*1e9) : 0),
is_valid_(_ts >= 0)
TimeStamp::TimeStamp(const double& _ts)
: time_stamp_nano_(_ts > 0 ? (unsigned long int)(_ts * 1e9) : 0), is_valid_(_ts >= 0)
{
//
}
TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec) :
time_stamp_nano_(_sec*NANOSECS+_nsec),
is_valid_(true)
TimeStamp::TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec)
: time_stamp_nano_(_sec * NANOSECS + _nsec), is_valid_(true)
{
//
}
TimeStamp::~TimeStamp()
{
//nothing to do
// nothing to do
}
void TimeStamp::setToNow()
......@@ -83,30 +74,30 @@ void TimeStamp::setToNow()
setOk();
}
TimeStamp TimeStamp::operator +(const double& dt) const
TimeStamp TimeStamp::operator+(const double& dt) const
{
TimeStamp ts(*this);
ts += dt;
return ts;
}
TimeStamp TimeStamp::operator -(const double& dt) const
TimeStamp TimeStamp::operator-(const double& dt) const
{
TimeStamp ts(*this);
ts -= dt;
return ts;
}
void TimeStamp::operator -=(const double& dt)
void TimeStamp::operator-=(const double& dt)
{
unsigned long int dt_nano = (unsigned long int)(dt*NANOSECS);
is_valid_ = (time_stamp_nano_ >= dt_nano);
time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
unsigned long int dt_nano = (unsigned long int)(dt * NANOSECS);
is_valid_ = (time_stamp_nano_ >= dt_nano);
time_stamp_nano_ = (dt_nano > time_stamp_nano_ ? 0 : time_stamp_nano_ - dt_nano);
}
void TimeStamp::print(std::ostream & ost) const
void TimeStamp::print(std::ostream& ost) const
{
ost << *this;
}
} // namespace wolf
} // namespace wolf
src/composite/prior_composite.cpp 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/composite/prior_composite.h"
namespace wolf
{
Prior::Prior(const std::string& _prior_mode, const Eigen::VectorXd& _factor_std)
: prior_mode_(_prior_mode), factor_std_(_factor_std)
{
Prior::check();
}
Prior::Prior(const YAML::Node& _n)
{
prior_mode_ = _n["mode"].as<std::string>();
if (prior_mode_ == "factor")
factor_std_ = _n["factor_std"].as<Eigen::VectorXd>();
else
factor_std_ = Eigen::VectorXd(0);
Prior::check();
}
void Prior::check() const
{
if (prior_mode_ != "initial_guess" and prior_mode_ != "fix" and prior_mode_ != "factor")
throw std::runtime_error("Prior::check() wrong prior_mode_ value: " + prior_mode_ +
", it should be: 'initial_guess', 'fix' or 'factor'. \n");
}
std::ostream& operator<<(std::ostream& _os, const wolf::Prior& _x)
{
if (_x.getPriorMode() == "factor")
_os << "mode: " + _x.getPriorMode() << " factor_std: " << _x.getFactorStd().transpose();
else
_os << "mode: " + _x.getPriorMode();
return _os;
}
YAML::Node Prior::toYaml() const
{
YAML::Node node;
node["mode"] = prior_mode_;
if (prior_mode_ == "factor") node["factor_std"] = factor_std_;
return node;
}
} // namespace wolf
src/composite/vector_composite.cpp 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/composite/vector_composite.h"
using namespace Eigen;
namespace wolf
{
VectorComposite::VectorComposite(const Composite<Eigen::VectorXd>& _composite) : Composite<VectorXd>(_composite) {}
VectorXd VectorComposite::vector(const StateKeys& _keys) const
{
// traverse once with unordered_map access
std::vector<const VectorXd*> vp;
unsigned int size = 0;
for (const auto& key : _keys)
{
vp.push_back(&(this->at(key)));
size += vp.back()->size();
}
VectorXd x(size);
// traverse once linearly
unsigned int index = 0;
for (const auto& blkp : vp)
{
x.segment(index, blkp->size()) = *blkp;
index += blkp->size();
}
return x;
}
std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x)
{
for (auto&& pair : _x)
{
_os << pair.first << ": (" << pair.second.transpose() << "); ";
}
return _os;
}
} // namespace wolf
src/factor/factor_analytic.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/factor/factor_analytic.h"
#include "core/state_block/state_block.h"
namespace wolf {
FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
FactorAnalytic(_tp,
_top,
_feature_ptr,
_frame_other_ptr ? FrameBasePtrList({_frame_other_ptr}) : FrameBasePtrList(),
_capture_other_ptr ? CaptureBasePtrList({_capture_other_ptr}) : CaptureBasePtrList(),
_feature_other_ptr ? FeatureBasePtrList({_feature_other_ptr}) : FeatureBasePtrList(),
_landmark_other_ptr ? LandmarkBasePtrList({_landmark_other_ptr}) : LandmarkBasePtrList(),
_processor_ptr,
_apply_loss_function,
_status,
_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr){}
FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
FactorBase(_tp,
_top,
_feature_ptr,
_frame_other_list,
_capture_other_list,
_feature_other_list,
_landmark_other_list,
_processor_ptr,
_apply_loss_function,
_status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_ptr_const_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
_state1Ptr ? (unsigned int) _state1Ptr->getSize() : 0,
_state2Ptr ? (unsigned int) _state2Ptr->getSize() : 0,
_state3Ptr ? (unsigned int) _state3Ptr->getSize() : 0,
_state4Ptr ? (unsigned int) _state4Ptr->getSize() : 0,
_state5Ptr ? (unsigned int) _state5Ptr->getSize() : 0,
_state6Ptr ? (unsigned int) _state6Ptr->getSize() : 0,
_state7Ptr ? (unsigned int) _state7Ptr->getSize() : 0,
_state8Ptr ? (unsigned int) _state8Ptr->getSize() : 0,
_state9Ptr ? (unsigned int) _state9Ptr->getSize() : 0})
namespace wolf
{
resizeVectors();
}
std::vector<StateBlockConstPtr> FactorAnalytic::getStateBlockPtrVector() const
FactorAnalytic::FactorAnalytic(const std::string& _type,
const FactorTopology& _top,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtrList& _node_state_block_list,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
FactorStatus _status)
: FactorBase(_type,
_top,
JAC_ANALYTIC,
_measurement,
_measurement_sqrt_information_upper,
_node_state_block_list,
_processor_ptr,
_state_ptrs,
_apply_loss_function,
_status)
{
return state_ptr_const_vector_;
}
std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector()
bool FactorAnalytic::evaluate(double const* const* parameters, double* residuals, double** jacobians) const
{
return state_ptr_vector_;
}
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
for (unsigned int i = 0; i < state_block_sizes_.size(); i++)
state_blocks_map_.emplace_back((double*)parameters[i], state_block_sizes_[i]);
std::vector<unsigned int> FactorAnalytic::getStateSizes() const
{
return state_block_sizes_vector_;
}
// residuals
Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
residuals_map = evaluateResiduals(state_blocks_map_);
JacobianMethod FactorAnalytic::getJacobianMethod() const
{
return JAC_ANALYTIC;
}
// also compute jacobians
if (jacobians != nullptr)
{
std::vector<Eigen::Map<Eigen::MatrixRowXd>> jacobians_map_;
std::vector<bool> compute_jacobians_(state_block_sizes_.size());
void FactorAnalytic::resizeVectors()
{
assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required");
for (unsigned int ii = 1; ii<state_ptr_vector_.size(); ii++)
if (state_ptr_vector_.at(ii) == nullptr)
for (unsigned int i = 0; i < state_block_sizes_.size(); i++)
{
state_ptr_vector_.resize(ii);
state_ptr_const_vector_.resize(ii);
state_block_sizes_vector_.resize(ii);
break;
compute_jacobians_[i] = (jacobians[i] != nullptr);
if (jacobians[i] != nullptr)
jacobians_map_.emplace_back((double*)jacobians[i], getSize(), state_block_sizes_[i]);
else
jacobians_map_.emplace_back(nullptr, 0, 0);
}
}
} // namespace wolf
// evaluate jacobians
evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
}
return true;
};
bool FactorAnalytic::evaluate(const std::vector<const double*>& _states_ptr,
Eigen::VectorXd& residual_,
std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(_states_ptr.size() == state_block_sizes_.size());
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
for (unsigned int i = 0; i < state_block_sizes_.size(); i++)
state_blocks_map_.emplace_back(_states_ptr[i], state_block_sizes_[i]);
// residuals
residual_ = evaluateResiduals(state_blocks_map_);
// compute jacobians
jacobians_.resize(state_block_sizes_.size());
evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_.size(), true));
return true;
};
} // namespace wolf
src/factor/factor_base.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/factor/factor_base.h"
#include "core/capture/capture_base.h"
#include "core/frame/frame_base.h"
#include "core/landmark/landmark_base.h"
#include "core/sensor/sensor_base.h"
namespace wolf {
unsigned int FactorBase::factor_id_count_ = 0;
FactorBase::FactorBase(const std::string& _tp,
const FactorTopology& _top,
const FeatureBasePtr& _feature_ptr,
const FrameBasePtrList& _frame_other_list,
const CaptureBasePtrList& _capture_other_list,
const FeatureBasePtrList& _feature_other_list,
const LandmarkBasePtrList& _landmark_other_list,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
FactorStatus _status) :
NodeBase("FACTOR", _tp),
feature_ptr_(), // will be filled in link()
processor_ptr_(_processor_ptr),
frame_other_list_(),
capture_other_list_(),
feature_other_list_(),
landmark_other_list_(),
factor_id_(++factor_id_count_),
topology_(_top),
status_(_status),
apply_loss_function_(_apply_loss_function)
namespace wolf
{
for (auto Fo : _frame_other_list)
frame_other_list_.push_back(Fo);
for (auto Co : _capture_other_list)
capture_other_list_.push_back(Co);
for (auto fo : _feature_other_list)
feature_other_list_.push_back(fo);
for (auto Lo : _landmark_other_list)
landmark_other_list_.push_back(Lo);
assert(_feature_ptr && "null feature pointer when creating a factor");
measurement_ = _feature_ptr->getMeasurement();
measurement_sqrt_information_upper_ = _feature_ptr->getMeasurementSquareRootInformationUpper();
}
unsigned int FactorBase::factor_id_count_ = 0;
void FactorBase::remove(bool viral_remove_empty_parent)
FactorBase::FactorBase(const std::string& _type,
const FactorTopology& _top,
const JacobianMethod& _jac_method,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _measurement_sqrt_information_upper,
const NodeStateBlocksPtrList& _node_state_block_list,
const ProcessorBasePtr& _processor_ptr,
const std::vector<StateBlockPtr>& _state_ptrs,
bool _apply_loss_function,
FactorStatus _status)
: NodeBase("FACTOR", _type),
feature_ptr_(),
processor_ptr_(_processor_ptr),
node_state_blocks_list_(),
factor_id_(++factor_id_count_),
topology_(_top),
jacobian_method_(_jac_method),
status_(_status),
apply_loss_function_(_apply_loss_function),
measurement_(_measurement),
measurement_sqrt_information_upper_(_measurement_sqrt_information_upper),
state_block_vector_(_state_ptrs)
{
/* 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_)
// build lists of nodes
for (auto Nsb : _node_state_block_list)
{
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
if (not Nsb) throw std::runtime_error("FactorBase constructor: nullptr in _node_state_block_list ");
if (find_if(node_state_blocks_list_.cbegin(),
node_state_blocks_list_.cend(),
[Nsb](const NodeStateBlocksConstWPtr& nsb_w) { return nsb_w.lock() == Nsb; }) !=
node_state_blocks_list_.end())
throw std::runtime_error("FactorBase constructor: repeated node in _node_state_block_list " +
Nsb->getCategory() + std::to_string(Nsb->id()));
node_state_blocks_list_.push_back(Nsb);
if (Nsb->isFrame())
frame_list_.push_back(std::dynamic_pointer_cast<FrameBase>(Nsb));
else if (Nsb->isCapture())
capture_list_.push_back(std::dynamic_pointer_cast<CaptureBase>(Nsb));
else if (Nsb->isLandmark())
landmark_list_.push_back(std::dynamic_pointer_cast<LandmarkBase>(Nsb));
else if (Nsb->isSensor())
sensor_list_.push_back(std::dynamic_pointer_cast<SensorBase>(Nsb));
}
// add factor to be removed from solver
if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE)
getProblem()->notifyFactor(this_fac,REMOVE);
// remove null state blocks at the end
while (not state_block_vector_.back()) state_block_vector_.pop_back();
// remove from upstream
FeatureBasePtr f = feature_ptr_.lock();
if (f)
{
f->removeFactor(this_fac);
if (viral_remove_empty_parent && f->getFactorList().empty() && f->getConstrainedByList().empty())
f->remove(viral_remove_empty_parent); // remove upstream
}
// store sizes
for (auto sb_it = state_block_vector_.begin(); sb_it != state_block_vector_.end(); sb_it++)
{
StateBlockPtr sb = *sb_it;
if (not sb)
throw std::runtime_error(
"FactorBase constructor: any nullptr in _state_ptrs (nullptrs at the back were already removed)");
if (std::find(state_block_vector_.begin(), sb_it, sb) != sb_it)
throw std::runtime_error("FactorBase constructor: repeated state block in _state_ptrs");
state_block_sizes_.push_back(sb->getSize());
}
// remove other: {Frame, Capture, Feature, Landmark}
for (auto& frm_ow : frame_other_list_)
// check that all nodes have any state block
for (auto Nsb : _node_state_block_list)
{
bool sb_found = false;
for (auto sb : state_block_vector_)
{
auto frm_o = frm_ow.lock();
if (frm_o)
if (Nsb->hasStateBlock(sb))
{
frm_o->removeConstrainedBy(this_fac);
if (viral_remove_empty_parent && frm_o->getConstrainedByList().empty() && frm_o->getCaptureList().empty())
frm_o->remove(viral_remove_empty_parent);
}
sb_found = true;
break;
};
}
if (not sb_found)
throw std::runtime_error(
"FactorBase constructor: a node in _node_state_block_list does not have any of the factored state "
"blocks.");
}
for (auto& cap_ow : capture_other_list_)
// check that all state blocks belong to a node
for (auto sb : state_block_vector_)
{
bool node_found = false;
for (auto Nsb : _node_state_block_list)
{
auto cap_o = cap_ow.lock();
if (cap_o)
if (Nsb->hasStateBlock(sb))
{
cap_o->removeConstrainedBy(this_fac);
if (viral_remove_empty_parent && cap_o->getConstrainedByList().empty() && cap_o->getFeatureList().empty())
cap_o->remove(viral_remove_empty_parent);
}
node_found = true;
break;
};
}
if (not node_found)
throw std::runtime_error(
"FactorBase constructor: a state block in _state_block_vector does not belong to any of the nodes in "
"_node_state_block_list.");
}
}
for (auto& ftr_ow : feature_other_list_)
Eigen::VectorXd FactorBase::residual() const
{
Eigen::VectorXd residual(getSize());
std::vector<double*> states_data;
for (auto&& sb : state_block_vector_) states_data.push_back(sb->getStateData());
evaluate(states_data.data(), residual.data(), nullptr);
return residual;
}
void FactorBase::remove(bool viral_remove_parent_without_children)
{
/* 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_) return;
is_removing_ = true;
FactorBasePtr this_fac = shared_from_this(); // keep this alive while removing it
// add factor to be removed from solver
if (getProblem() != nullptr and this->getStatus() == FAC_ACTIVE) getProblem()->notifyFactor(this_fac, REMOVE);
// remove from upstream
FeatureBasePtr f = feature_ptr_.lock();
if (f)
{
f->removeFactor(this_fac);
if (viral_remove_parent_without_children and not f->hasChildren())
{
auto ftr_o = ftr_ow.lock();
if (ftr_o)
{
ftr_o->removeConstrainedBy(this_fac);
if (viral_remove_empty_parent && ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove(viral_remove_empty_parent);
}
f->remove(viral_remove_parent_without_children); // remove upstream
}
for (auto& lmk_ow : landmark_other_list_)
}
// remove from factored nodes
for (auto& node_states_w : node_state_blocks_list_)
{
auto node = node_states_w.lock();
if (node)
{
auto lmk_o = lmk_ow.lock();
if (lmk_o)
node->removeFactor(this_fac);
if (viral_remove_parent_without_children and not node->isSensor() and not node->hasChildren())
{
lmk_o->removeConstrainedBy(this_fac);
if (viral_remove_empty_parent && lmk_o->getConstrainedByList().empty())
lmk_o->remove(viral_remove_empty_parent);
node->remove(viral_remove_parent_without_children);
}
}
// std::cout << "Removed c" << id() << std::endl;
}
node_state_blocks_list_.clear();
capture_list_.clear();
frame_list_.clear();
landmark_list_.clear();
sensor_list_.clear();
// std::cout << "Removed c" << id() << std::endl;
}
CaptureBaseConstPtr FactorBase::getCapture() const
{
auto ftr = getFeature();
if (ftr != nullptr and not ftr->isRemoving())
if (ftr != nullptr and not ftr->isRemoving())
return ftr->getCapture();
else return nullptr;
else
return nullptr;
}
CaptureBasePtr FactorBase::getCapture()
{
auto ftr = getFeature();
if (ftr != nullptr and not ftr->isRemoving())
if (ftr != nullptr and not ftr->isRemoving())
return ftr->getCapture();
else return nullptr;
else
return nullptr;
}
FrameBaseConstPtr FactorBase::getFrame() const
{
auto cpt = getCapture();
if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
if (cpt != nullptr and not cpt->isRemoving())
return cpt->getFrame();
else
return nullptr;
}
FrameBasePtr FactorBase::getFrame()
{
auto cpt = getCapture();
if(cpt != nullptr and not cpt->isRemoving()) return cpt->getFrame();
else return nullptr;
}
SensorBaseConstPtr FactorBase::getSensor() const
{
auto cpt = getCapture();
if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
}
SensorBasePtr FactorBase::getSensor()
{
auto cpt = getCapture();
if(cpt != nullptr and not cpt->isRemoving()) return cpt->getSensor();
else return nullptr;
if (cpt != nullptr and not cpt->isRemoving())
return cpt->getFrame();
else
return nullptr;
}
void FactorBase::setStatus(FactorStatus _status)
......@@ -189,67 +227,39 @@ void FactorBase::setStatus(FactorStatus _status)
else if (_status != status_)
{
if (_status == FAC_ACTIVE)
getProblem()->notifyFactor(shared_from_this(),ADD);
getProblem()->notifyFactor(shared_from_this(), ADD);
else if (_status == FAC_INACTIVE)
getProblem()->notifyFactor(shared_from_this(),REMOVE);
getProblem()->notifyFactor(shared_from_this(), REMOVE);
}
status_ = _status;
}
bool FactorBase::hasFrameOther(const FrameBaseConstPtr &_frm_other) const
bool FactorBase::hasNode(const NodeStateBlocksConstPtr& _nsb) const
{
auto nsb_it = find_if(node_state_blocks_list_.cbegin(),
node_state_blocks_list_.cend(),
[_nsb](const NodeStateBlocksConstWPtr& nsb_w) { return nsb_w.lock() == _nsb; });
return (nsb_it != node_state_blocks_list_.cend());
}
bool FactorBase::hasFrame(const FrameBaseConstPtr& _frm) const
{
auto frm_it = find_if(frame_other_list_.begin(),
frame_other_list_.end(),
[_frm_other](const FrameBaseConstWPtr &frm_ow)
{
return frm_ow.lock() == _frm_other;
});
if (frm_it != frame_other_list_.end())
return true;
return false;
return hasNode(std::static_pointer_cast<const NodeStateBlocks>(_frm));
}
bool FactorBase::hasCaptureOther(const CaptureBaseConstPtr &_cap_other) const
bool FactorBase::hasCapture(const CaptureBaseConstPtr& _cap) const
{
auto cap_it = find_if(capture_other_list_.begin(),
capture_other_list_.end(),
[_cap_other](const CaptureBaseConstWPtr &cap_ow)
{
return cap_ow.lock() == _cap_other;
});
if (cap_it != capture_other_list_.end())
return true;
return false;
return hasNode(std::static_pointer_cast<const NodeStateBlocks>(_cap));
}
bool FactorBase::hasFeatureOther(const FeatureBaseConstPtr &_ftr_other) const
bool FactorBase::hasLandmark(const LandmarkBaseConstPtr& _lmk) const
{
auto ftr_it = find_if(feature_other_list_.begin(),
feature_other_list_.end(),
[_ftr_other](const FeatureBaseConstWPtr &ftr_ow)
{
return ftr_ow.lock() == _ftr_other;
});
if (ftr_it != feature_other_list_.end())
return true;
return false;
return hasNode(std::static_pointer_cast<const NodeStateBlocks>(_lmk));
}
bool FactorBase::hasLandmarkOther(const LandmarkBaseConstPtr &_lmk_other) const
bool FactorBase::hasSensor(const SensorBaseConstPtr& _sen) const
{
auto lmk_it = find_if(landmark_other_list_.begin(),
landmark_other_list_.end(),
[_lmk_other](const LandmarkBaseConstWPtr &lmk_ow)
{
return lmk_ow.lock() == _lmk_other;
});
if (lmk_it != landmark_other_list_.end())
return true;
return false;
return hasNode(std::static_pointer_cast<const NodeStateBlocks>(_sen));
}
void FactorBase::link(FeatureBasePtr _ftr_ptr)
......@@ -258,337 +268,169 @@ void FactorBase::link(FeatureBasePtr _ftr_ptr)
assert(this->getFeature() == nullptr && "linking an already linked factor");
// not link if nullptr
if(_ftr_ptr == nullptr)
WOLF_DEBUG_COND(_ftr_ptr == nullptr, "Factor of type ", getType(), " id: ", id(), " _ftr_ptr is nullptr");
if (_ftr_ptr)
{
WOLF_WARN("Linking with nullptr");
return;
// link with feature
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr,
"Adding factor ",
this->id(),
" to feature ",
_ftr_ptr->id(),
" that is NOT connected with problem.");
this->setProblem(_ftr_ptr->getProblem());
}
// link with feature
_ftr_ptr->addFactor(shared_from_this());
this->setFeature(_ftr_ptr);
// set problem ( and register factor )
WOLF_WARN_COND(_ftr_ptr->getProblem() == nullptr, "ADDING FACTOR ", this->id(), " TO FEATURE ", _ftr_ptr->id(), " THAT IS NOT CONNECTED WITH PROBLEM.");
this->setProblem(_ftr_ptr->getProblem());
// constrained by
for (auto& frm_ow : frame_other_list_)
// constrained node_state_blocks
for (auto& node_w : node_state_blocks_list_)
{
auto frame_other = frm_ow.lock();
if(frame_other != nullptr)
auto node = node_w.lock();
WOLF_WARN_COND(node == nullptr,
"Factor of type ",
getType(),
" id: ",
id(),
" node_state_blocks weak pointer is NOT valid.");
if (node != nullptr)
{
assert(frame_other->getProblem() && "Forbidden: linking a factor with a floating frame_other.");
frame_other->addConstrainedBy(shared_from_this());
WOLF_DEBUG_COND(node->getProblem() == nullptr,
"Factor of type ",
getType(),
" id: ",
id(),
" node_state_blocks getProblem() is nullptr.");
if (not getProblem()) this->setProblem(node->getProblem());
node->addFactor(shared_from_this());
}
}
for (auto& cap_ow : capture_other_list_)
{
auto capture_other = cap_ow.lock();
if(capture_other != nullptr) capture_other->addConstrainedBy(shared_from_this());
}
for (auto& ftr_ow : feature_other_list_)
{
auto feature_other = ftr_ow.lock();
if(feature_other != nullptr) feature_other->addConstrainedBy(shared_from_this());
}
for (auto& lmk_ow : landmark_other_list_)
{
auto landmark_other = lmk_ow.lock();
if(landmark_other != nullptr) landmark_other->addConstrainedBy(shared_from_this());
}
}
void FactorBase::setProblem(ProblemPtr _problem)
{
if (_problem == nullptr || _problem == this->getProblem())
return;
if (_problem == nullptr || _problem == this->getProblem()) return;
NodeBase::setProblem(_problem);
if (this->getStatus() == FAC_ACTIVE)
this->getProblem()->notifyFactor(shared_from_this(),ADD);
if (this->getStatus() == FAC_ACTIVE) this->getProblem()->notifyFactor(shared_from_this(), ADD);
}
void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
void FactorBase::printHeader(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
_stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " -->";
if ( getFrameOtherList() .empty()
&& getCaptureOtherList() .empty()
&& getFeatureOtherList() .empty()
&& getLandmarkOtherList().empty())
_stream << " Abs";
for (auto Fow : getFrameOtherList())
if (!Fow.expired())
_stream << " Frm" << Fow.lock()->id();
for (auto Cow : getCaptureOtherList())
if (!Cow.expired())
_stream << " Cap" << Cow.lock()->id();
for (auto fow : getFeatureOtherList())
if (!fow.expired())
_stream << " Ftr" << fow.lock()->id();
for (auto Low : getLandmarkOtherList())
if (!Low.expired())
_stream << " Lmk" << Low.lock()->id();
_stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " --> ";
_stream << topology_;
for (auto Nw : getNodesFactored())
if (!Nw.expired())
{
const auto& N = Nw.lock();
_stream << " " << N->getCategory() << N->id();
continue;
}
_stream << std::endl;
}
void FactorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
void FactorBase::print(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
}
CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std::ostream& _stream, std::string _tabs) const
CheckLog FactorBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
CheckLog log;
std::stringstream inconsistency_explanation;
if (_verbose)
_stream << _tabs << "Fac" << id() << " @ " << _fac_ptr.get();
if (_verbose) _stream << _tabs << "Fac" << id() << " @ " << this;
if ( getFrameOtherList() .empty()
&& getCaptureOtherList() .empty()
&& getFeatureOtherList() .empty()
&& getLandmarkOtherList().empty() ) // case ABSOLUTE:
// find factored_by pointer in factored nodes
for (auto Nw : getNodesFactored())
{
if (_verbose)
_stream << " --> Abs.";
}
// find constrained_by pointer in constrained frame
for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
if (not Nw.expired())
{
const auto& Fo = Fow.lock();
const auto& N = Nw.lock();
if (_verbose)
{
_stream << " ( --> Frm" << Fo->id() << " <- ";
for (auto cby : Fo->getConstrainedByList())
_stream << " Fac" << cby->id();
}
_stream << " ( " << N->getCategory() << N->id() << " <- ";
// check constrained_by pointer in constrained frame
bool found = Fo->isConstrainedBy(_fac_ptr);
inconsistency_explanation << "The constrained Feature " << Fo->id() << " @ " << Fo
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
for (auto fby : N->getFactoredBySet()) _stream << " Fac" << fby->id();
}
}
if (_verbose && !getFrameOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained capture
for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
const auto& Co = Cow.lock();
if (_verbose)
{
_stream << " ( --> Cap" << Co->id() << " <- ";
for (auto cby : Co->getConstrainedByList())
_stream << " Fac" << cby->id();
_stream << ")";
}
// check constrained_by pointer in constrained frame
bool found = Co->isConstrainedBy(_fac_ptr);
inconsistency_explanation << "The constrained capture " << Co->id() << " @ " << Co
<< " not found among constrained-by factors\n";
// check factored_by pointer in factored node
bool found = N->hasFactor(shared_from_this());
inconsistency_explanation << "Fac" << id() << " was not found among the factored-by factors of "
<< N->getCategory() << N->id() << " @ " << N << std::endl;
log.assertTrue((found), inconsistency_explanation);
}
}
if (_verbose && !getCaptureOtherList().empty())
_stream << ")";
// find constrained_by pointer in constrained feature
for (auto fow : getFeatureOtherList())
{
if (!fow.expired())
{
const auto& fo = fow.lock();
if (_verbose)
{
_stream << " ( --> Ftr" << fo->id() << " <- ";
for (auto cby : fo->getConstrainedByList())
_stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained feature
bool found = fo->isConstrainedBy(_fac_ptr);
inconsistency_explanation << "The constrained feature " << fo->id() << " @ " << fo
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
}
}
if (_verbose && !getFeatureOtherList().empty())
_stream << ")";
if (_verbose) _stream << std::endl;
// find constrained_by pointer in constrained landmark
for (auto Low : getLandmarkOtherList())
// Check Problem and feature ptrs
auto ftr_ptr = getFeature();
if (ftr_ptr)
{
if (Low.expired())
if (_verbose)
{
const auto& Lo = Low.lock();
if (_verbose)
{
_stream << " ( --> Lmk" << Lo->id() << " <- ";
for (auto cby : Lo->getConstrainedByList())
_stream << " Fac" << cby->id();
}
// check constrained_by pointer in constrained landmark
bool found = Lo->isConstrainedBy(_fac_ptr);
inconsistency_explanation << "The constrained landmark " << Lo->id() << " @ " << Lo
<< " not found among constrained-by factors\n";
log.assertTrue((found), inconsistency_explanation);
_stream << _tabs << " "
<< "-> Prb @ " << getProblem() << std::endl;
_stream << _tabs << " "
<< "-> Ftr" << getFeature()->id() << " @ " << getFeature() << std::endl;
}
}
if (_verbose && !getLandmarkOtherList().empty())
_stream << ")";
if (_verbose)
_stream << std::endl;
//Check Problem and feature ptrs
if (_verbose)
{
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Ftr" << getFeature()->id() << " @ " << getFeature().get() << std::endl;
// check problem and feature pointers
inconsistency_explanation << "The factor " << id() << " @ " << this << " problem ptr " << getProblem()
<< " is different from Feature's problem ptr " << ftr_ptr->getProblem() << "\n";
log.assertTrue((getProblem() == ftr_ptr->getProblem()), inconsistency_explanation);
inconsistency_explanation << "Fac" << id() << " @ " << this << " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
<< " -X-> Fac" << id();
auto ftr_fac_list = ftr_ptr->getFactorList();
auto _this_fac = shared_from_this();
auto ftr_has_fac = std::find_if(ftr_fac_list.cbegin(),
ftr_fac_list.cend(),
[_this_fac](FactorBaseConstPtr fac) { return fac == _this_fac; });
log.assertTrue(ftr_has_fac != ftr_fac_list.cend(), inconsistency_explanation);
}
auto ftr_ptr = getFeature();
// check problem and feature pointers
inconsistency_explanation << "The factor " << id() << " @ " << _fac_ptr.get() << " problem ptr " << getProblem().get()
<< " is different from Feature's problem ptr " << ftr_ptr->getProblem().get() << "\n";
log.assertTrue((getProblem() == ftr_ptr->getProblem()), inconsistency_explanation);
inconsistency_explanation << "Fac" << id() << " @ " << _fac_ptr
<< " ---> Ftr" << ftr_ptr->id() << " @ " << ftr_ptr
<< " -X-> Fac" << id();
auto ftr_fac_list = ftr_ptr->getFactorList();
auto ftr_has_fac = std::find_if(ftr_fac_list.begin(), ftr_fac_list.end(), [&_fac_ptr](FactorBaseConstPtr fac){ return fac == _fac_ptr;});
log.assertTrue(ftr_has_fac!= ftr_fac_list.end(), inconsistency_explanation);
// find state block pointers in all constrained nodes
auto S = getSensor(); // get own sensor to check sb
auto F = getFrame();
auto C = getCapture();
// find state block pointers in all factored nodes
for (auto sb : getStateBlockPtrVector())
{
bool found = false;
if (_verbose)
{
_stream << _tabs << " " << "sb @ " << sb.get();
_stream << _tabs << " "
<< "sb @ " << sb;
if (sb)
{
auto lp = sb->getLocalParametrization();
if (lp)
_stream << " (lp @ " << lp.get() << ")";
if (lp) _stream << " (lp @ " << lp << ")";
}
}
bool found_here;
// find in own Frame
found_here = F->hasStateBlock(sb);
if (found_here && _verbose) _stream << " Frm" << F->id();
found = found || found_here;
// find in own Capture
found_here = C->hasStateBlock(sb);
if (found_here && _verbose) _stream << " Cap" << 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 << " Frm" << F->id() << ".Cap" << FC->id();
found = found || found_here;
}
// find in own Sensor
if (S)
{
found_here = S->hasStateBlock(sb);
if (found_here && _verbose) _stream << " Sen" << S->id();
found = found || found_here;
}
// find in constrained Frame
for (auto Fow : getFrameOtherList())
{
if (!Fow.expired())
{
const auto& Fo = Fow.lock();
found_here = Fo->hasStateBlock(sb);
if (found_here && _verbose) _stream << " FrmO" << 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 << " FrmO" << Fo->id() << ".C" << FoC->id();
found = found || found_here;
}
}
}
// find in constrained Capture
for (auto Cow : getCaptureOtherList())
{
if (!Cow.expired())
{
const auto& Co = Cow.lock();
found_here = Co->hasStateBlock(sb);
if (found_here && _verbose) _stream << " CapO" << Co->id();
found = found || found_here;
}
}
// find in constrained Feature
for (auto fow : 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 << " FtrOF" << 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 << " FtrOC" << 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 << " FtrOS" << foS->id();
found = found || found_here;
}
}
// find in constrained landmark
for (auto Low : getLandmarkOtherList())
// find in factored node
for (auto Nw : getNodesFactored())
{
if (!Low.expired())
if (!Nw.expired())
{
const auto& Lo = Low.lock();
found_here = Lo->hasStateBlock(sb);
if (found_here && _verbose) _stream << " LmkO" << Lo->id();
found = found || found_here;
const auto& N = Nw.lock();
found_here = N->hasStateBlock(sb);
if (found_here && _verbose) _stream << " " << N->getCategory() << N->id();
found = found || found_here;
}
}
......@@ -605,18 +447,17 @@ CheckLog FactorBase::localCheck(bool _verbose, FactorBaseConstPtr _fac_ptr, std:
inconsistency_explanation << "The stateblock " << sb << " has not been found (is floating!)";
log.assertTrue((found), inconsistency_explanation);
inconsistency_explanation << "The stateblock " << sb << " of factor " << id() << " @ " << _fac_ptr << " is null\n";
log.assertTrue((sb.get() != nullptr), inconsistency_explanation);
inconsistency_explanation << "The stateblock " << sb << " of factor " << id() << " @ " << this << " is null\n";
log.assertTrue((sb != nullptr), inconsistency_explanation);
}
return log;
}
bool FactorBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
bool FactorBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto fac_ptr = std::static_pointer_cast<const FactorBase>(_node_ptr);
auto local_log = localCheck(_verbose, fac_ptr, _stream, _tabs);
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
return _log.is_consistent_;
}
} // namespace wolf
} // namespace wolf
src/feature/feature_base.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_base.h"
#include "core/factor/factor_base.h"
#include "core/capture/capture_base.h"
#include "core/math/covariance.h"
namespace wolf {
namespace wolf
{
unsigned int FeatureBase::feature_id_count_ = 0;
FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_uncertainty, UncertaintyType _uncertainty_type) :
NodeBase("FEATURE", _type),
capture_ptr_(),
feature_id_(++feature_id_count_),
track_id_(0),
landmark_id_(0),
measurement_(_measurement)
FeatureBase::FeatureBase(const std::string& _type,
const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _meas_uncertainty,
UncertaintyType _uncertainty_type)
: NodeBase("FEATURE", _type),
capture_ptr_(),
feature_id_(++feature_id_count_),
track_id_(0),
landmark_id_(0),
measurement_(_measurement)
{
switch (_uncertainty_type)
{
case UNCERTAINTY_IS_INFO :
case UNCERTAINTY_IS_INFO:
setMeasurementInformation(_meas_uncertainty);
break;
case UNCERTAINTY_IS_COVARIANCE :
case UNCERTAINTY_IS_COVARIANCE:
setMeasurementCovariance(_meas_uncertainty);
break;
case UNCERTAINTY_IS_STDDEV :
case UNCERTAINTY_IS_STDDEV:
WOLF_ERROR("STDEV case Not implemented yet");
break;
default :
default:
break;
}
// std::cout << "constructed +f" << id() << std::endl;
}
FeatureBase::~FeatureBase()
{
// std::cout << "destructed -f" << id() << std::endl;
}
void FeatureBase::remove(bool viral_remove_empty_parent)
void FeatureBase::remove(bool viral_remove_parent_without_children)
{
/* 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_)
{
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
if (is_removing_) return;
// remove downstream
while (!factor_list_.empty())
{
factor_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
}
is_removing_ = true;
FeatureBasePtr this_f = shared_from_this(); // keep this alive while removing it
// remove from upstream
CaptureBasePtr C = capture_ptr_.lock();
if (C)
// remove downstream
while (not factor_list_.empty())
{
factor_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
}
// remove from upstream
CaptureBasePtr C = capture_ptr_.lock();
if (C)
{
C->removeFeature(this_f); // remove from upstream
if (viral_remove_parent_without_children and not C->hasChildren())
{
C->removeFeature(this_f); // remove from upstream
if (viral_remove_empty_parent && C->getFeatureList().empty() && C->getConstrainedByList().empty())
C->remove(viral_remove_empty_parent); // remove upstream
C->remove(viral_remove_parent_without_children); // remove upstream
}
}
}
FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
{
factor_list_.push_back(_co_ptr);
return _co_ptr;
}
void FeatureBase::removeFactor(FactorBasePtr _co_ptr)
{
factor_list_.remove(_co_ptr);
}
FrameBaseConstPtr FeatureBase::getFrame() const
{
return capture_ptr_.lock()->getFrame();
}
FrameBasePtr FeatureBase::getFrame()
FactorBasePtr FeatureBase::addFactor(FactorBasePtr _fac)
{
return capture_ptr_.lock()->getFrame();
factor_list_.push_back(_fac);
return _fac;
}
FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
void FeatureBase::removeFactor(FactorBasePtr _fac)
{
constrained_by_list_.push_back(_fac_ptr);
return _fac_ptr;
factor_list_.remove(_fac);
}
void FeatureBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
bool FeatureBase::hasFactor(FactorBaseConstPtr _fac) const
{
constrained_by_list_.remove(_fac_ptr);
return std::find(factor_list_.begin(), factor_list_.end(), _fac) != factor_list_.end();
}
bool FeatureBase::isConstrainedBy(const FactorBaseConstPtr &_factor) const
void FeatureBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
return std::find(constrained_by_list_.begin(),
constrained_by_list_.end(),
_factor) != constrained_by_list_.end();
for (auto&& obj_ptr : factor_list_) _fac_list.push_back(obj_ptr);
}
bool FeatureBase::hasFactor(FactorBaseConstPtr _factor) const
void FeatureBase::getFactorList(FactorBasePtrList& _fac_list)
{
return std::find(factor_list_.begin(),
factor_list_.end(),
_factor) != factor_list_.end();
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
void FeatureBase::getFactorList(FactorBaseConstPtrList & _fac_list) const
FrameBaseConstPtr FeatureBase::getFrame() const
{
// FIXME
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
return capture_ptr_.lock()->getFrame();
}
void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
FrameBasePtr FeatureBase::getFrame()
{
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
return capture_ptr_.lock()->getFrame();
}
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov)
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd& _meas_cov)
{
WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
// set (ensuring symmetry)
measurement_covariance_ = _meas_cov.selfadjointView<Eigen::Upper>();
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
// compute square root information upper matrix
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd& _meas_info)
{
WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
......@@ -178,19 +149,12 @@ void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
measurement_ = _meas;
}
void FeatureBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
for (auto fac : factor_list_)
fac->setProblem(_problem);
}
void FeatureBase::link(CaptureBasePtr _cpt_ptr)
{
assert(!is_removing_ && "linking a removed feature");
assert(this->getCapture() == nullptr && "linking an already linked feature");
if(_cpt_ptr)
if (_cpt_ptr)
{
_cpt_ptr->addFeature(shared_from_this());
this->setCapture(_cpt_ptr);
......@@ -201,97 +165,78 @@ void FeatureBase::link(CaptureBasePtr _cpt_ptr)
WOLF_WARN("Linking with nullptr");
}
}
void FeatureBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
void FeatureBase::printHeader(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
_stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType() << ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "");
if (_constr_by)
{
_stream << " <--\t";
for (auto cby : getConstrainedByList())
if (cby)
_stream << "Fac" << cby->id() << " \t";
}
_stream << std::endl;
_stream << _tabs << "Ftr" << id() << " trk" << trackId() << " " << getType()
<< ((_depth < 4) ? " -- " + std::to_string(getFactorList().size()) + "fac " : "") << std::endl;
if (_metric)
_stream << _tabs << " " << "m = ( " << std::setprecision(2) << getMeasurement().transpose()
<< " )" << std::endl;
_stream << _tabs << " "
<< "m = ( " << std::setprecision(2) << getMeasurement().transpose() << " )" << std::endl;
}
void FeatureBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
void FeatureBase::print(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 4)
for (auto c : getFactorList())
if (c)
c->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
if (c) c->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog FeatureBase::localCheck(bool _verbose, FeatureBaseConstPtr _ftr_ptr, std::ostream& _stream, std::string _tabs) const
CheckLog FeatureBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
CheckLog log;
std::stringstream inconsistency_explanation;
if (_verbose)
{
_stream << _tabs << "Ftr" << id() << " @ " << _ftr_ptr.get() << std::endl;
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Cap" << getCapture()->id() << " @ " << getCapture().get()
<< std::endl;
_stream << _tabs << "Ftr" << id() << " @ " << this << std::endl;
_stream << _tabs << " "
<< "-> Prb @ " << getProblem() << std::endl;
_stream << _tabs << " "
<< "-> Cap" << getCapture()->id() << " @ " << getCapture() << std::endl;
}
auto cap_ptr = getCapture();
// check problem and capture pointers
inconsistency_explanation << "Feature frame problem pointer " << getProblem().get()
<< " different from Capture problem pointer " << cap_ptr->getProblem().get() << "\n";
inconsistency_explanation << "Feature frame problem pointer " << getProblem()
<< " different from Capture problem pointer " << cap_ptr->getProblem() << "\n";
log.assertTrue((getProblem() == cap_ptr->getProblem()), inconsistency_explanation);
// check contrained_by
for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
for (auto fow : cby->getFeatureOtherList())
_stream << " Ftr" << fow.lock()->id();
_stream << std::endl;
}
// check constrained_by pointer to this feature
inconsistency_explanation << "constrained by Feature " << id() << " @ " << _ftr_ptr.get()
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasFeatureOther(_ftr_ptr)), inconsistency_explanation);
}
// Check capture
auto cap_ftr = _ftr_ptr->getCapture();
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
<< " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
auto cap_ftr = getCapture();
inconsistency_explanation << "Ftr" << id() << " @ " << this << " ---> Cap" << cap_ftr->id() << " @ " << cap_ftr
<< " -X-> Ftr" << id();
auto cap_ftr_list = cap_ftr->getFeatureList();
auto frame_has_cap = std::find(cap_ftr_list.begin(), cap_ftr_list.end(), _ftr_ptr);
auto cap_ftr_list = cap_ftr->getFeatureList();
auto frame_has_cap = std::find(cap_ftr_list.begin(), cap_ftr_list.end(), shared_from_this());
log.assertTrue(frame_has_cap != cap_ftr_list.end(), inconsistency_explanation);
// Check factors
for(auto fac : getFactorList())
for (auto fac : getFactorList())
{
inconsistency_explanation << "Ftr" << id() << " @ " << _ftr_ptr
<< " ---> Fac" << fac->id() << " @ " << fac
inconsistency_explanation << "Ftr" << id() << " @ " << this << " ---> Fac" << fac->id() << " @ " << fac
<< " -X-> Ftr" << id();
log.assertTrue((fac->getFeature() == _ftr_ptr), inconsistency_explanation);
log.assertTrue((fac->getFeature() == shared_from_this()), inconsistency_explanation);
}
return log;
}
bool FeatureBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
bool FeatureBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto ftr_ptr = std::static_pointer_cast<const FeatureBase>(_node_ptr);
auto local_log = localCheck(_verbose, ftr_ptr, _stream, _tabs);
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
for(auto f : getFactorList()) f->check(_log, f, _verbose, _stream, _tabs + " ");
for (auto f : getFactorList()) f->check(_log, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
} // namespace wolf
} // namespace wolf
src/feature/feature_diff_drive.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_diff_drive.h"
namespace wolf
{
FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
const Eigen::MatrixXd& _delta_preintegrated_covariance,
const Eigen::VectorXd& _diff_drive_params,
const Eigen::MatrixXd& _jacobian_diff_drive_params) :
FeatureMotion("FeatureDiffDrive",
_delta_preintegrated,
_delta_preintegrated_covariance,
_diff_drive_params,
_jacobian_diff_drive_params)
const Eigen::MatrixXd& _jacobian_diff_drive_params)
: FeatureMotion("FeatureDiffDrive",
_delta_preintegrated,
_delta_preintegrated_covariance,
_diff_drive_params,
_jacobian_diff_drive_params)
{
//
}
......
src/feature/feature_landmark_external.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_landmark_external.h"
namespace wolf
{
FeatureLandmarkExternal::FeatureLandmarkExternal(const Eigen::VectorXd& _measurement,
const Eigen::MatrixXd& _meas_covariance,
const int& _external_id,
......
src/feature/feature_motion.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* feature_motion.cpp
*
* Created on: Aug 11, 2017
* Author: jsola
*/
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_motion.h"
namespace wolf
{
FeatureMotion::FeatureMotion(const std::string& _type,
const VectorXd& _delta_preint,
const MatrixXd _delta_preint_cov,
const VectorXd& _calib_preint,
const MatrixXd& _jacobian_calib) :
FeatureBase(_type, _delta_preint, _delta_preint_cov),
calib_preint_(_calib_preint),
jacobian_calib_(_jacobian_calib)
const VectorXd& _delta_preint,
const MatrixXd _delta_preint_cov,
const VectorXd& _calib_preint,
const MatrixXd& _jacobian_calib)
: FeatureBase(_type, _delta_preint, _delta_preint_cov),
calib_preint_(_calib_preint),
jacobian_calib_(_jacobian_calib)
{
//
}
......
src/feature/feature_odom_2d.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_odom_2d.h"
namespace wolf {
FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
namespace wolf
{
FeatureOdom2d::FeatureOdom2d(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
: FeatureBase("FeatureOdom2d", _measurement, _meas_covariance)
{
//std::cout << "New FeatureOdom2d: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
}
FeatureOdom2d::~FeatureOdom2d()
......@@ -34,4 +30,4 @@ FeatureOdom2d::~FeatureOdom2d()
//
}
} // namespace wolf
} // namespace wolf
src/feature/feature_pose.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/feature/feature_pose.h"
namespace wolf {
FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("FeaturePose", _measurement, _meas_covariance)
namespace wolf
{
FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance)
: FeatureBase("FeaturePose", _measurement, _meas_covariance)
{
//
}
......@@ -34,4 +31,4 @@ FeaturePose::~FeaturePose()
//
}
} // namespace wolf
} // namespace wolf
src/frame/frame_base.cpp
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "core/frame/frame_base.h"
#include "core/factor/factor_base.h"
#include "core/trajectory/trajectory_base.h"
......@@ -29,115 +25,53 @@
#include "core/state_block/state_quaternion.h"
#include "core/state_block/factory_state_block.h"
namespace wolf {
namespace wolf
{
unsigned int FrameBase::frame_id_count_ = 0;
FrameBase::FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const VectorComposite& _state) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
FrameBase::FrameBase(const TimeStamp& _ts,
const TypeComposite& _frame_types,
const VectorComposite& _frame_vectors,
const PriorComposite& _frame_priors)
: NodeStateBlocks("FRAME", "FrameBase", _frame_types, _frame_vectors, _frame_priors),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
{
assert(_state.includesStructure(_frame_structure) && "_state does not include all keys of _frame_structure");
for (auto key : getStructure())
{
StateBlockPtr sb = FactoryStateBlock::create(string(1,key), _state.at(key), false); // false = non fixed
addStateBlock(key, sb, getProblem());
}
}
FrameBase::FrameBase(const TimeStamp& _ts,
const StateStructure& _frame_structure,
const std::list<VectorXd>& _vectors) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(_frame_structure),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
void FrameBase::remove(bool viral_remove_parent_without_children)
{
assert(_frame_structure.size() == _vectors.size() && "Structure size does not match number of provided vectors!");
auto vec_it = _vectors.begin();
for (const auto key : _frame_structure)
{
const auto& vec = *vec_it;
StateBlockPtr sb = FactoryStateBlock::create(string(1,key), vec, false); // false = non fixed
addStateBlock(key, sb, getProblem());
vec_it++;
}
}
/* 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_) return;
is_removing_ = true;
FrameBasePtr this_F = shared_from_this_frame(); // keep this alive while removing it
FrameBase::FrameBase(const TimeStamp& _ts,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _v_ptr) :
NodeBase("FRAME", "FrameBase"),
HasStateBlocks(""),
trajectory_ptr_(),
frame_id_(++frame_id_count_),
time_stamp_(_ts)
{
if (_p_ptr)
// remove downstream
while (!capture_list_.empty())
{
addStateBlock('P', _p_ptr, getProblem());
capture_list_.front()->remove(viral_remove_parent_without_children); // remove downstream
}
if (_o_ptr)
{
addStateBlock('O', _o_ptr, getProblem());
}
if (_v_ptr)
// remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
if (T)
{
addStateBlock('V', _v_ptr, getProblem());
T->removeFrame(this_F); // remove from upstream
}
}
FrameBase::~FrameBase()
{
// Remove Frame State Blocks
NodeStateBlocks::remove(viral_remove_parent_without_children);
}
void FrameBase::remove(bool viral_remove_empty_parent)
bool FrameBase::hasChildren() const
{
/* 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_)
{
is_removing_ = true;
FrameBasePtr this_F = 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
}
while (!capture_list_.empty())
{
capture_list_.front()->remove(viral_remove_empty_parent); // remove downstream
}
// Remove Frame State Blocks
removeStateBlocks(getProblem());
// remove from upstream
TrajectoryBasePtr T = trajectory_ptr_.lock();
if (T)
{
T->removeFrame(this_F); // remove from upstream
}
}
return NodeStateBlocks::hasChildren() or not capture_list_.empty();
}
void FrameBase::setTimeStamp(const TimeStamp& _ts)
......@@ -151,43 +85,42 @@ void FrameBase::link(ProblemPtr _prb)
this->link(_prb->getTrajectory());
}
bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
{
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
return getTrajectory()->getPreviousFrame(shared_from_this(), i);
return getTrajectory()->getPreviousFrame(shared_from_this_frame(), i);
}
FrameBasePtr FrameBase::getPreviousFrame(const unsigned int& i)
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
return getTrajectory()->getPreviousFrame(shared_from_this(), i);
return getTrajectory()->getPreviousFrame(shared_from_this_frame(), i);
}
FrameBaseConstPtr FrameBase::getNextFrame(const unsigned int& i) const
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
return getTrajectory()->getNextFrame(shared_from_this(), i);
return getTrajectory()->getNextFrame(shared_from_this_frame(), i);
}
FrameBasePtr FrameBase::getNextFrame(const unsigned int& i)
{
assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
return getTrajectory()->getNextFrame(shared_from_this(), i);
return getTrajectory()->getNextFrame(shared_from_this_frame(), i);
}
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id());
capture_list_.push_back(_capt_ptr);
WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr,
"Adding new capture ",
_capt_ptr->id(),
" in a frame with another capture of the same sensor: ",
getCaptureOf(_capt_ptr->getSensor())->id());
capture_list_.push_back(_capt_ptr);
return _capt_ptr;
}
......@@ -199,16 +132,14 @@ void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
CaptureBaseConstPtr FrameBase::getCaptureOfType(const std::string& type) const
{
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
return capture_ptr;
if (capture_ptr->getType() == type) return capture_ptr;
return nullptr;
}
CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type)
{
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
return capture_ptr;
if (capture_ptr->getType() == type) return capture_ptr;
return nullptr;
}
......@@ -216,8 +147,7 @@ CaptureBaseConstPtrList FrameBase::getCapturesOfType(const std::string& type) co
{
CaptureBaseConstPtrList captures;
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
captures.push_back(capture_ptr);
if (capture_ptr->getType() == type) captures.push_back(capture_ptr);
return captures;
}
......@@ -225,24 +155,21 @@ CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type)
{
CaptureBasePtrList captures;
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getType() == type)
captures.push_back(capture_ptr);
if (capture_ptr->getType() == type) captures.push_back(capture_ptr);
return captures;
}
CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr, const std::string& type) const
{
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type) return capture_ptr;
return nullptr;
}
CaptureBasePtr FrameBase::getCaptureOf(SensorBasePtr _sensor_ptr, const std::string& type)
{
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type) return capture_ptr;
return nullptr;
}
......@@ -250,8 +177,7 @@ CaptureBaseConstPtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr) cons
{
if (_sensor_ptr)
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
if (capture_ptr->getSensor() == _sensor_ptr) return capture_ptr;
return nullptr;
}
......@@ -259,8 +185,7 @@ CaptureBasePtr FrameBase::getCaptureOf(SensorBaseConstPtr _sensor_ptr)
{
if (_sensor_ptr)
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
if (capture_ptr->getSensor() == _sensor_ptr) return capture_ptr;
return nullptr;
}
......@@ -268,8 +193,7 @@ CaptureBaseConstPtrList FrameBase::getCapturesOf(SensorBaseConstPtr _sensor_ptr)
{
CaptureBaseConstPtrList captures;
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
if (capture_ptr->getSensor() == _sensor_ptr) captures.push_back(capture_ptr);
return captures;
}
......@@ -277,63 +201,10 @@ CaptureBasePtrList FrameBase::getCapturesOf(SensorBasePtr _sensor_ptr)
{
CaptureBasePtrList captures;
for (auto capture_ptr : getCaptureList())
if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
if (capture_ptr->getSensor() == _sensor_ptr) captures.push_back(capture_ptr);
return captures;
}
FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type) const
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
return nullptr;
}
FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr, const std::string& type)
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
return nullptr;
}
FactorBaseConstPtr FrameBase::getFactorOf(ProcessorBaseConstPtr _processor_ptr) const
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
return nullptr;
}
FactorBasePtr FrameBase::getFactorOf(ProcessorBasePtr _processor_ptr)
{
for (auto factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
for (auto factor_ptr : getFactorList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
return nullptr;
}
FactorBaseConstPtrList FrameBase::getFactorList() const
{
FactorBaseConstPtrList fac_list;
......@@ -351,40 +222,18 @@ FactorBasePtrList FrameBase::getFactorList()
void FrameBase::getFactorList(FactorBaseConstPtrList& _fac_list) const
{
for (auto c_ptr : getCaptureList())
if (not c_ptr->isRemoving())
c_ptr->getFactorList(_fac_list);
if (not c_ptr->isRemoving()) c_ptr->getFactorList(_fac_list);
}
void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
if (not c_ptr->isRemoving())
c_ptr->getFactorList(_fac_list);
if (not c_ptr->isRemoving()) c_ptr->getFactorList(_fac_list);
}
bool FrameBase::hasCapture(const CaptureBaseConstPtr& _capture) const
{
return std::find(capture_list_.begin(),
capture_list_.end(),
_capture) != capture_list_.end();
}
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 FactorBaseConstPtr &_factor) const
{
return std::find(constrained_by_list_.begin(),
constrained_by_list_.end(),
_factor) != constrained_by_list_.end();
return std::find(capture_list_.begin(), capture_list_.end(), _capture) != capture_list_.end();
}
void FrameBase::link(TrajectoryBasePtr _trj_ptr)
......@@ -392,9 +241,9 @@ 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)
if (_trj_ptr)
{
_trj_ptr->addFrame(shared_from_this());
_trj_ptr->addFrame(shared_from_this_frame());
this->setTrajectory(_trj_ptr);
this->setProblem(_trj_ptr->getProblem());
}
......@@ -406,140 +255,94 @@ void FrameBase::link(TrajectoryBasePtr _trj_ptr)
void FrameBase::setProblem(ProblemPtr _problem)
{
if (_problem == nullptr || _problem == this->getProblem())
return;
if (_problem == nullptr || _problem == this->getProblem()) return;
NodeBase::setProblem(_problem);
registerNewStateBlocks(_problem);
NodeStateBlocks::setProblem(_problem);
for (auto cap : capture_list_)
cap->setProblem(_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
void FrameBase::printHeader(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
_stream << _tabs << "Frm" << id()
<< " " << getStructure()
<< " ts = " << std::setprecision(3) << getTimeStamp()
_stream << _tabs << "Frm" << id() << " " << getKeys() << " ts = " << std::setprecision(3) << getTimeStamp()
<< ((_depth < 2) ? " -- " + std::to_string(getCaptureList().size()) + "C " : "");
if (_constr_by)
if (_factored_by)
{
_stream << " <-- ";
for (auto cby : getConstrainedByList())
if (cby)
_stream << "Fac" << cby->id() << " \t";
for (auto fac : getFactoredBySet())
if (fac) _stream << "Fac" << fac->id() << " \t";
}
_stream << std::endl;
printState(_metric, _state_blocks, _stream, _tabs);
printState(_factored_by, _metric, _state_blocks, _stream, _tabs);
}
void FrameBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
void FrameBase::print(int _depth,
bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
printHeader(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs);
if (_depth >= 2)
for (auto C : getCaptureList())
if (C)
C->print(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs + " ");
if (C) C->print(_depth, _factored_by, _metric, _state_blocks, _stream, _tabs + " ");
}
CheckLog FrameBase::localCheck(bool _verbose, FrameBaseConstPtr _frm_ptr, std::ostream& _stream, std::string _tabs) const
CheckLog FrameBase::localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const
{
CheckLog log;
CheckLog log;
std::stringstream inconsistency_explanation;
if (_verbose) {
_stream << _tabs << "Frm"
<< id() << " @ " << _frm_ptr.get() << std::endl;
_stream << _tabs << " " << "-> Prb @ " << getProblem().get() << std::endl;
_stream << _tabs << " " << "-> Trj @ " << getTrajectory().get() << std::endl;
}
for (const auto &pair: getStateBlockMap())
if (_verbose)
{
auto sb = pair.second;
// check for valid state block
inconsistency_explanation << "Frame " << id() << " @ "<< _frm_ptr.get()
<< " has State block pointer " << sb.get()
<< " = 0 \n";
log.assertTrue((sb.get() != 0), inconsistency_explanation);
if (_verbose) {
_stream << _tabs << " " << pair.first << " sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
_stream << " (lp @ " << lp.get() << ")";
}
_stream << std::endl;
}
_stream << _tabs << "Frm" << id() << " @ " << this << std::endl;
_stream << _tabs << " "
<< "-> Prb @ " << getProblem() << std::endl;
_stream << _tabs << " "
<< "-> Trj @ " << getTrajectory() << std::endl;
}
// check problem pointer
auto trajectory_ptr = getTrajectory();
auto trajectory_ptr = getTrajectory();
auto trajectory_problem_ptr = trajectory_ptr->getProblem();
inconsistency_explanation << "Frame problem pointer " << getProblem().get()
<< " different from Trajectory problem pointer " << trajectory_problem_ptr.get() << "\n";
inconsistency_explanation << "Frame problem pointer " << getProblem()
<< " different from Trajectory problem pointer " << trajectory_problem_ptr << "\n";
log.assertTrue((getProblem() == trajectory_problem_ptr), inconsistency_explanation);
// check constrained_by
for (auto cby : getConstrainedByList())
{
if (_verbose)
{
_stream << _tabs << " " << "<- Fac" << cby->id() << " -> ";
for (auto Fow : cby->getFrameOtherList())
_stream << " Frm" << Fow.lock()->id() << std::endl;
// check constrained_by pointer to this frame
inconsistency_explanation << "constrained-by frame " << id() << " @ " << _frm_ptr
<< " not found among constrained-by factors\n";
log.assertTrue((cby->hasFrameOther(_frm_ptr)), inconsistency_explanation);
for (auto sb : cby->getStateBlockPtrVector())
{
if (_verbose) {
_stream << _tabs << " " << "sb @ " << sb.get();
if (sb) {
auto lp = sb->getLocalParametrization();
if (lp)
_stream << " (lp @ " << lp.get() << ")";
}
_stream << std::endl;
}
}
}
}
// Trajectory
auto trj_ptr = getTrajectory();
inconsistency_explanation << "Frm" << id() << " @ " << _frm_ptr
<< " ---> Trj" << " @ " << trj_ptr
<< " -X-> Frm" << id();
log.assertTrue(trj_ptr->hasFrame(_frm_ptr), inconsistency_explanation);
inconsistency_explanation << "Frm" << id() << " @ " << this << " ---> Trj"
<< " @ " << trj_ptr << " -X-> Frm" << id();
log.assertTrue(trj_ptr->hasFrame(shared_from_this_frame()), inconsistency_explanation);
// Captures
for(auto C : getCaptureList())
for (auto C : getCaptureList())
{
inconsistency_explanation << "Frm " << id() << " @ " << _frm_ptr
<< " ---> Cap" << C->id() << " @ " << C
inconsistency_explanation << "Frm " << id() << " @ " << this << " ---> Cap" << C->id() << " @ " << C
<< " -X-> Frm" << id();
log.assertTrue((C->getFrame() == _frm_ptr), inconsistency_explanation);
log.assertTrue((C->getFrame() == shared_from_this_frame()), inconsistency_explanation);
}
// check NodeStateBlocks
auto node_log = NodeStateBlocks::localCheck(_verbose, _stream, _tabs);
log.compose(node_log);
return log;
}
bool FrameBase::check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs) const
bool FrameBase::check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs) const
{
auto frm_ptr = std::static_pointer_cast<const FrameBase>(_node_ptr);
auto local_log = localCheck(_verbose, frm_ptr, _stream, _tabs);
auto local_log = localCheck(_verbose, _stream, _tabs);
_log.compose(local_log);
for(auto C : getCaptureList())
C->check(_log, C, _verbose, _stream, _tabs + " ");
for (auto C : getCaptureList()) C->check(_log, _verbose, _stream, _tabs + " ");
return _log.is_consistent_;
}
} // namespace wolf
} // namespace wolf