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
Showing
with 1941 additions and 1413 deletions
//--------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--------
#ifndef TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
#define TRUNK_SRC_COST_FUNCTION_WRAPPER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// WOLF
#include "core/common/wolf.h"
......@@ -32,42 +28,41 @@
// EIGEN
#include <Eigen/StdVector>
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(CostFunctionWrapper);
class CostFunctionWrapper : public ceres::CostFunction
{
private:
FactorBasePtr factor_ptr_;
public:
private:
FactorBasePtr factor_ptr_;
CostFunctionWrapper(FactorBasePtr _factor_ptr);
public:
CostFunctionWrapper(FactorBasePtr _factor_ptr);
~CostFunctionWrapper() override;
~CostFunctionWrapper() override;
bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const override;
bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override;
FactorBasePtr getFactor() const;
FactorBasePtr getFactor() const;
};
inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) :
ceres::CostFunction(), factor_ptr_(_factor_ptr)
inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr)
: ceres::CostFunction(), factor_ptr_(_factor_ptr)
{
for (auto st_block_size : factor_ptr_->getStateSizes())
mutable_parameter_block_sizes()->push_back(st_block_size);
for (auto st_block_size : factor_ptr_->getStateSizes()) mutable_parameter_block_sizes()->push_back(st_block_size);
set_num_residuals(factor_ptr_->getSize());
}
inline CostFunctionWrapper::~CostFunctionWrapper()
{
}
inline CostFunctionWrapper::~CostFunctionWrapper() {}
inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, double* residuals, double** jacobians) const
inline bool CostFunctionWrapper::Evaluate(const double* const* parameters, double* residuals, double** jacobians) const
{
return factor_ptr_->evaluate(parameters, residuals, jacobians);
auto res = factor_ptr_->evaluate(parameters, residuals, jacobians);
WOLF_ERROR_COND(not res, "Error evaluating factor ", factor_ptr_->getType(), ": ", factor_ptr_->id());
return res;
}
inline FactorBasePtr CostFunctionWrapper::getFactor() const
......@@ -75,6 +70,4 @@ inline FactorBasePtr CostFunctionWrapper::getFactor() const
return factor_ptr_;
}
} // namespace wolf
#endif /* TRUNK_SRC_COST_FUNCTION_WRAPPER_H_ */
} // namespace wolf
//--------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--------
/*
* create_numeric_diff_cost_function.h
*
* Created on: Apr 5, 2016
* Author: jvallve
*/
#ifndef SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
#define SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "ceres/cost_function.h"
#include "ceres/numeric_diff_cost_function.h"
......@@ -35,37 +24,58 @@
// Factors
#include "core/factor/factor_base.h"
namespace wolf {
namespace wolf
{
// Wolf ceres auto_diff creator
template <class T>
std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
std::shared_ptr<ceres::NumericDiffCostFunction<T,
ceres::CENTRAL,
T::residualSize,
T::block0Size,
T::block1Size,
T::block2Size,
T::block3Size,
T::block4Size,
T::block5Size,
T::block6Size,
T::block7Size,
T::block8Size,
T::block9Size> >
createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
{
return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get());
return std::make_shared<ceres::NumericDiffCostFunction<T,
ceres::CENTRAL,
T::residualSize,
T::block0Size,
T::block1Size,
T::block2Size,
T::block3Size,
T::block4Size,
T::block5Size,
T::block6Size,
T::block7Size,
T::block8Size,
T::block9Size> >(
std::static_pointer_cast<T>(_factor_ptr).get());
};
inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr)
{
// switch (_fac_ptr->getTypeId())
// {
// // just for testing
// case FAC_ODOM_2d:
// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
//
// /* For adding a new factor, add the #include and a case:
// case FAC_ENUM:
// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
// */
//
// default:
throw std::invalid_argument( "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
// }
// switch (_fac_ptr->getTypeId())
// {
// // just for testing
// case FAC_ODOM_2d:
// return createNumericDiffCostFunctionCeres<FactorOdom2d>(_fac_ptr);
//
// /* For adding a new factor, add the #include and a case:
// case FAC_ENUM:
// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
// */
//
// default:
throw std::invalid_argument(
"Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h");
// }
}
} // namespace wolf
#endif /* SRC_CERES_WRAPPER_CREATE_NUMERIC_DIFF_COST_FUNCTION_H_ */
} // namespace wolf
//--------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--------
/*
* iteration_callback.h
*
* Created on: Jun 15, 2020
* Author: joanvallve
*/
#ifndef INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
#define INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/problem/problem.h"
#include "ceres/ceres.h"
namespace wolf {
namespace wolf
{
class IterationUpdateCallback : public ceres::IterationCallback
{
public:
explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
: problem_(_problem)
, min_num_iterations_(min_num_iterations)
, initial_cost_(0)
, verbose_(verbose)
{}
public:
explicit IterationUpdateCallback(ProblemPtr _problem, const int& min_num_iterations, bool verbose = false)
: problem_(_problem), min_num_iterations_(min_num_iterations), initial_cost_(0), verbose_(verbose)
{
}
~IterationUpdateCallback() {}
~IterationUpdateCallback() {}
ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
{
if (summary.iteration == 0)
initial_cost_ = summary.cost;
ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override
{
if (summary.iteration == 0)
initial_cost_ = summary.cost;
else if (summary.iteration >= min_num_iterations_ and
(problem_->getStateBlockNotificationMapSize() != 0 or
problem_->getFactorNotificationMapSize() != 0))
else if (summary.iteration >= min_num_iterations_ and
(problem_->getStateBlockNotificationMapSize() != 0 or problem_->getFactorNotificationMapSize() != 0))
{
if (summary.cost >= initial_cost_)
{
if (summary.cost >= initial_cost_)
{
WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ", summary.iteration);
return ceres::SOLVER_ABORT;
}
else
{
WOLF_INFO_COND(verbose_, "Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ", summary.iteration);
return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
}
WOLF_INFO_COND(
verbose_,
"Stopping solver to update the problem! ABORTING since cost didn't decrease. Iteration ",
summary.iteration);
return ceres::SOLVER_ABORT;
}
else
{
WOLF_INFO_COND(verbose_,
"Stopping solver to update the problem! SUCCESS since cost decreased. Iteration ",
summary.iteration);
return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
}
return ceres::SOLVER_CONTINUE;
}
return ceres::SOLVER_CONTINUE;
}
private:
ProblemPtr problem_;
int min_num_iterations_;
double initial_cost_;
bool verbose_;
private:
ProblemPtr problem_;
int min_num_iterations_;
double initial_cost_;
bool verbose_;
};
}
#endif /* INCLUDE_CORE_CERES_WRAPPER_ITERATION_UPDATE_CALLBACK_H_ */
} // namespace wolf
//--------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--------
#ifndef LOCAL_PARAMETRIZATION_WRAPPER_H_
#define LOCAL_PARAMETRIZATION_WRAPPER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd refs
namespace wolf{
namespace wolf
{
class LocalParametrizationBase;
}
//Ceres includes
// Ceres includes
#include "core/common/wolf.h"
#include "ceres/ceres.h"
namespace wolf {
namespace wolf
{
class LocalParametrizationWrapper : public ceres::LocalParameterization
{
private:
LocalParametrizationBasePtr local_parametrization_ptr_;
private:
LocalParametrizationBasePtr local_parametrization_ptr_;
public:
public:
LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr);
~LocalParametrizationWrapper() override = default;
~LocalParametrizationWrapper() override = default;
bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
bool Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const override;
bool ComputeJacobian(const double* x, double* jacobian) const override;
bool ComputeJacobian(const double* x, double* jacobian) const override;
int GlobalSize() const override;
int GlobalSize() const override;
int LocalSize() const override;
int LocalSize() const override;
LocalParametrizationBasePtr getLocalParametrization() const;
LocalParametrizationBasePtr getLocalParametrization() const;
};
using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
} // namespace wolf
} // namespace wolf
#include "core/state_block/local_parametrization_base.h"
namespace wolf {
inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr) :
local_parametrization_ptr_(_local_parametrization_ptr)
namespace wolf
{
inline LocalParametrizationWrapper::LocalParametrizationWrapper(LocalParametrizationBasePtr _local_parametrization_ptr)
: local_parametrization_ptr_(_local_parametrization_ptr)
{
}
......@@ -83,6 +79,4 @@ inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametr
return local_parametrization_ptr_;
}
} // namespace wolf
#endif
} // namespace wolf
//--------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.h
*
* Created on: Jun 7, 2017
* Author: jvallve
*/
#ifndef SRC_CERES_WRAPPER_QR_MANAGER_H_
#define SRC_CERES_WRAPPER_QR_MANAGER_H_
#include "core/solver/solver_manager.h"
#include "core/solver_suitesparse/sparse_utils.h"
namespace wolf
{
class QRManager : public SolverManager
{
protected:
Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
Eigen::SparseMatrixd A_;
Eigen::VectorXd b_;
std::map<StateBlockPtr, unsigned int> sb_2_col_;
std::map<FactorBasePtr, unsigned int> fac_2_row_;
bool any_state_block_removed_;
unsigned int new_state_blocks_;
unsigned int N_batch_;
bool pending_changes_;
public:
QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch);
virtual ~QRManager();
virtual std::string solve(const unsigned int& _report_level);
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
virtual void computeCovariances(const std::vector<StateBlockPtr>& _sb_list);
private:
bool computeDecomposition();
virtual void addFactor(FactorBasePtr _fac_ptr);
virtual void removeFactor(FactorBasePtr _fac_ptr);
virtual void addStateBlock(StateBlockPtr _st_ptr);
virtual void removeStateBlock(StateBlockPtr _st_ptr);
virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
void relinearizeFactor(FactorBasePtr _fac_ptr);
};
} /* namespace wolf */
#endif /* SRC_CERES_WRAPPER_QR_MANAGER_H_ */
//--------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--------
#ifndef CERES_MANAGER_H_
#define CERES_MANAGER_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
//Ceres includes
// Ceres includes
#include "ceres/jet.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
//wolf includes
// wolf includes
#include "core/solver/solver_manager.h"
#include "core/utils/params_server.h"
#include "yaml-cpp/yaml.h"
namespace ceres {
typedef std::shared_ptr<CostFunction> CostFunctionPtr;
namespace ceres
{
typedef std::shared_ptr<CostFunction> CostFunctionPtr;
}
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(SolverCeres);
WOLF_PTR_TYPEDEFS(LocalParametrizationWrapper);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsCeres);
struct ParamsCeres : public ParamsSolver
{
bool interrupt_on_problem_change = false;
int min_num_iterations = 1;
ceres::Solver::Options solver_options;
ceres::Problem::Options problem_options;
ceres::Covariance::Options covariance_options;
ParamsCeres() :
ParamsSolver()
{
loadHardcodedValues();
}
ParamsCeres(std::string _unique_name, const ParamsServer& _server) :
ParamsSolver(_unique_name, _server)
{
loadHardcodedValues();
// interrupt solver whenever the problem is updated (via ceres::iterationCallback)
interrupt_on_problem_change = _server.getParam<bool>(prefix + "interrupt_on_problem_change");
if (interrupt_on_problem_change)
min_num_iterations = _server.getParam<int>(prefix + "min_num_iterations");
// CERES SOLVER OPTIONS
solver_options.max_num_iterations = _server.getParam<int>(prefix + "max_num_iterations");
solver_options.function_tolerance = _server.getParam<double>(prefix + "function_tolerance");
solver_options.gradient_tolerance = _server.getParam<double>(prefix + "gradient_tolerance");
solver_options.num_threads = _server.getParam<int>(prefix + "n_threads");
covariance_options.num_threads = solver_options.num_threads;
// minimizer type
std::string minimizer = _server.getParam<std::string>(prefix + "minimizer");
if (minimizer == "LEVENBERG_MARQUARDT" or minimizer == "levenberg_marquardt")
{
solver_options.minimizer_type = ceres::TRUST_REGION;
solver_options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
}
else if (minimizer == "DOGLEG" or minimizer == "dogleg")
{
solver_options.minimizer_type = ceres::TRUST_REGION;
solver_options.trust_region_strategy_type = ceres::DOGLEG;
}
else if (minimizer == "LBFGS" or minimizer == "lbfgs")
{
solver_options.minimizer_type = ceres::LINE_SEARCH;
solver_options.line_search_direction_type = ceres::LBFGS;
}
else if (minimizer == "BFGS" or 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' (upper or lowercase)");
}
// specific options for TRUST REGION
if (solver_options.minimizer_type == ceres::TRUST_REGION)
{
solver_options.use_nonmonotonic_steps = _server.getParam<bool>(prefix + "use_nonmonotonic_steps");
if (solver_options.use_nonmonotonic_steps)
solver_options.max_consecutive_nonmonotonic_steps = _server.getParam<int>(prefix + "max_consecutive_nonmonotonic_steps");
}
}
void loadHardcodedValues()
{
solver_options = ceres::Solver::Options();
problem_options = ceres::Problem::Options();
covariance_options = ceres::Covariance::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;
covariance_options.apply_loss_function = false;
}
~ParamsCeres() override = default;
};
class SolverCeres : public SolverManager
{
protected:
std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
// this map is only for handling automatic destruction of localParametrizationWrapper objects
std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
protected:
std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
ceres::Solver::Summary summary_;
std::unique_ptr<ceres::Problem> ceres_problem_;
std::unique_ptr<ceres::Covariance> covariance_;
// this map is only for handling automatic destruction of localParametrizationWrapper objects
std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
ParamsCeresPtr params_ceres_;
ceres::Solver::Summary summary_;
std::unique_ptr<ceres::Problem> ceres_problem_;
std::unique_ptr<ceres::Covariance> covariance_;
// profiling
unsigned int n_iter_acc_, n_iter_max_;
unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
// profiling
unsigned int n_iter_acc_, n_iter_max_;
unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
public:
public:
SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params);
SolverCeres(const ProblemPtr& _wolf_problem);
WOLF_SOLVER_CREATE(SolverCeres);
SolverCeres(const ProblemPtr& _wolf_problem,
const ParamsCeresPtr& _params);
~SolverCeres() override;
WOLF_SOLVER_CREATE(SolverCeres, ParamsCeres);
ceres::Solver::Summary getSummary() const;
~SolverCeres() override;
std::unique_ptr<ceres::Problem>& getCeresProblem();
ceres::Solver::Summary getSummary();
bool computeCovariancesDerived(
CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
std::unique_ptr<ceres::Problem>& getCeresProblem();
bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
bool computeCovariancesDerived(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
bool converged() const override;
bool failed() const override;
bool wasStopped() const override;
unsigned int iterations() const override;
double initialCost() const override;
double finalCost() const override;
double totalTime() const override;
bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
ceres::Solver::Options& getSolverOptions();
bool hasConverged() override;
bool wasStopped() override;
unsigned int iterations() override;
double initialCost() override;
double finalCost() override;
double totalTime() override;
const Eigen::SparseMatrixd computeHessian() const;
ceres::Solver::Options& getSolverOptions();
virtual int numStateBlocksDerived() const override;
const Eigen::SparseMatrixd computeHessian() const;
virtual int numFactorsDerived() const override;
virtual int numStateBlocksDerived() const override;
protected:
bool checkDerived(std::string prefix = "") const override;
virtual int numFactorsDerived() const override;
std::string solveDerived(const ReportVerbosity report_level) override;
protected:
void addFactorDerived(const FactorBasePtr& fac_ptr) override;
bool checkDerived(std::string prefix="") const override;
void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
std::string solveDerived(const ReportVerbosity report_level) override;
void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
void addFactorDerived(const FactorBasePtr& fac_ptr) override;
void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
bool isStateBlockFixedDerived(const StateBlockPtr& st) const override;
bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) const override;
bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
bool isStateBlockFixedDerived(const StateBlockPtr& st) override;
void printProfilingDerived(std::ostream& _stream) const override;
bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) override;
bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
void printProfilingDerived(std::ostream& _stream) const override;
// PARAMS
ceres::Solver::Options solver_options_;
};
inline ceres::Solver::Summary SolverCeres::getSummary()
inline ceres::Solver::Summary SolverCeres::getSummary() const
{
return summary_;
}
......@@ -224,26 +134,23 @@ inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem()
inline ceres::Solver::Options& SolverCeres::getSolverOptions()
{
return params_ceres_->solver_options;
return solver_options_;
}
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
return fac_2_residual_idx_.count(fac_ptr) == 1 and
fac_2_costfunction_.count(fac_ptr) == 1;
return fac_2_residual_idx_.count(fac_ptr) == 1 and fac_2_costfunction_.count(fac_ptr) == 1;
}
inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
if (state_blocks_.count(state_ptr) == 0)
return false;
if (state_blocks_.count(state_ptr) == 0) return false;
return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st)
inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st) const
{
if (state_blocks_.count(st) == 0)
return false;
if (state_blocks_.count(st) == 0) return false;
return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
};
......@@ -262,6 +169,4 @@ inline int SolverCeres::numFactorsDerived() const
return ceres_problem_->NumResidualBlocks();
};
} // namespace wolf
#endif
} // namespace wolf
//--------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--------
/*
* sparse_utils.h
*
* Created on: Jul 2, 2015
* Author: jvallve
*/
#ifndef SPARSE_UTILS_H_
#define SPARSE_UTILS_H_
// eigen includes
//#include <Eigen/Sparse>
namespace wolf
{
void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.middleRows(_row,_n_rows) = Eigen::SparseMatrixd(_n_rows,A.cols());
A.makeCompressed();
}
void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.prune([&](int i, int, double) { return i >= _row && i < _row + _n_rows; });
A.makeCompressed();
}
void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
{
A.middleCols(_col,_n_cols) = Eigen::SparseMatrixd(A.rows(),_n_cols);
A.makeCompressed();
}
void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
{
A.prune([&](int, int j, double) { return j >= _col && j < _col + _n_cols; });
A.makeCompressed();
}
template<int _Options, typename _StorageIndex>
void assignSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& 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.coeffRef(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
original.makeCompressed();
}
template<int _Options, typename _StorageIndex>
void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& 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.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
original.makeCompressed();
}
template<int _Options, typename _StorageIndex>
void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& 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.makeCompressed();
}
void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen::SparseMatrix<double, Eigen::RowMajor>& ins, const unsigned int& _row)
{
assert(A.rows() >= _row + ins.rows() && A.cols() == ins.cols());
A.middleRows(_row, ins.rows()) = ins;
A.makeCompressed();
}
Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _diag_blocs)
{
unsigned int dim = _diag_blocs.front().rows();
unsigned int size = dim * _diag_blocs.size();
Eigen::SparseMatrixd M(size,size);
unsigned int pos = 0;
for (const Eigen::MatrixXd& omega_k : _diag_blocs)
{
insertSparseBlock(omega_k, M, pos, pos);
pos += dim;
}
M.makeCompressed();
return M;
}
template<int _Options, typename _StorageIndex>
void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXd>& diag_, const unsigned int& dim)
{
assert(_M.rows() % dim == 0 && "number of rows must be multiple of dimension");
diag_.clear();
for (auto i = 0; i < _M.rows(); i += dim)
diag_.push_back(Eigen::MatrixXd(_M.block(i,i,dim,dim)));
}
} // namespace wolf
#endif /* SPARSE_UTILS_H_ */
//--------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/>.
#pragma once
// CERES JET
......@@ -27,25 +23,15 @@
#include "core/math/rotations.h"
namespace ceres{
// // efficient implementation of pi2pi for jets (without while)
// template<int N>
// inline ceres::Jet<double, N> pi2pi(const ceres::Jet<double, N>& _angle)
// {
// ceres::Jet<double, N> angle = _angle;
// angle.a = pi2pi(_angle.a);
// return angle;
// }
// }
namespace ceres
{
using std::remainder;
template<int N>
template <int N>
inline Jet<double, N> remainder(const Jet<double, N>& _x, long double _y)
{
Jet<double, N> res = _x;
res.a = remainder(_x.a, _y);
res.a = remainder(_x.a, _y);
return res;
}
}
} // namespace ceres
//--------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--------
/**
* \file factory.h
*
* Created on: May 16, 2016
* \author: jsola
*/
#ifndef FACTORY_H_
#define FACTORY_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// wolf
#include "core/common/wolf.h"
#include "core/utils/unused.h"
// std
#include <string>
#include <map>
#include <list>
#include <iostream>
#include <iomanip>
// yaml
#include <yaml-cpp/yaml.h>
namespace wolf
{
/** \brief Singleton template factory
*
* This template class implements a generic factory as a singleton.
......@@ -55,46 +42,27 @@ namespace wolf
*
* - The class is templatized on the class of the produced objects, __TypeBase__.
* The produced objects are always of a class deriving from TypeBase.
* The returned data is always a shared pointer to TypeBase.
* The returned data is an object of type __TypeBase__. If you want a shared pointer to an object,
* then __TypeBase__ must be the desired shared pointer.
*
* From now on we will assume that a typedef is defined: ````std::shared_ptr<Type>```` as ````TypePtr````.
*
* For example, you may use as __TypeBase__ the following types:
* - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBasePtr```` to them
* - ParamsSensorBase: the Factory creates intrinsic parameters deriving from ParamsSensorBase and returns base pointers ````ParamsSensorBasePtr```` to them
* - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers
* ````LandmarkBasePtr```` to them
* - SensorBase: the Factory creates sensor deriving from SensorBase and returns base
* pointers ````SensorBasePtr```` to them
* - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them.
*
* - The class is variadic-templatized on the types of the input parameter of the creator, __TypeInput__:
* - ````std::string```` is used when the input parameter is a file name from which to read data (typically a YAML file).
* - ````std::string, wolf::ParamsServer``` is used when nodes are build from parameters in the ParamsServer
* - ````YAML::Node```` is used when the input parameter is a YAML node with structured data.
* - ````std::string```` is used when the input parameter is a file name from which to read data
* (typically a YAML file).
* - ````std::string, YAML::Node``` is used when nodes are build from parameters in a YAML node.
* - Any other variadic list of inputs is possible.
*
* - Examples of specific factories existing in Wolf are:
* \code
*
* // FactorySensor
* typedef Factory<SensorBase, // Type of objects to be returned: SensorBasePtr
* const std::string&, // Type of the sensor: name of the derived sensor class
* const Eigen::VectorXd&, // Sensor extrinsics
* const ParamsSensorBasePtr> // Sensor parameters
* FactorySensor;
*
* // FactoryProcessor
* typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
* const std::string&, // Type of the processor: name of the derived processor class
* const ParamsProcessorBasePtr> // Parameters for creating the processor
* FactoryProcessor;
*
* // AutoConfProcessorFactory
* typedef Factory<ProcessorBase, // Type of object returned: ProcessorBasePtr
* const std::string&, // Type of the processor: name of the derived processor class
* const ParamsServer> // Parameters for creating the processor
* AutoConfProcessorFactory;
*
* // Landmarks from YAML
* typedef Factory<LandmarkBase, // Type of node created: LandmarkBasePtr
* const YAML::Node&> // YAML node with the lmk params
* LandmarkFactory;
* \endcode
* - Examples of specific factories existing in Wolf are in ````factory_landmark.h````, ````factory_map.h````,
* ````factory_processor.h````, ````factory_sensor.h````, ````factory_solver.h````, ````factory_state_block.h````,
* ````factory_tree_manager.h````
*
* ### Operation of the factory
*
......@@ -102,13 +70,14 @@ namespace wolf
*
* Once specialized, this factory can create objects of classes deriving from TypeBase.
*
* > For example, if you want to make a Landmark factory, set TypeBase = LandmarkBase.\n
* > For example, if you want to make a Landmark factory, set TypeBase = LandmarkBasePtr.\n
* > Then, the factory will create specific landmarks deriving from LandmarkBase.\n
* > The specific type of landmark (e.g. LandmarkCorner2d, LandmarkAHP, LandmarkPolyline2d, etc) is indicated by a string that we call TYPE in this documentation.
* > The specific type of landmark (e.g. LandmarkCorner2d, LandmarkAHP, LandmarkPolyline2d, etc) is indicated by a
* string that we call TYPE in this documentation.
*
* Specific object creation is invoked by the method ````create(TYPE, params ... )````, where
* - the TYPE of object to create is identified with a string. This string matches the name of the derived class.
* - the params may be provided in different forms -- see TypeInput.
* - the params may be provided in different forms, normally YAML node -- see TypeInput.
*
* The methods to create specific objects are called __creators__.
* Creators must be registered to the factory before they can be invoked for object creation.
......@@ -123,9 +92,7 @@ namespace wolf
*
* #### Define correct TYPE names
* We use a std::string with literally the same text as the derived class name, e.g.:
* - ParamsSensorCamera -> ````"ParamsSensorCamera"````
* - SensorCamera -> ````"SensorCamera"````
* - ParamsSensorLaser2d -> ````"ParamsSensorLaser2d"````
* - SensorLaser2d -> ````"SensorLaser2d"````
* - LandmarkPolyline2d -> ````"LandmarkPolyline2d"````
* - etc.
......@@ -135,19 +102,14 @@ namespace wolf
* The first thing to know is that we have defined typedefs for the templates that we are using. For example:
*
* \code
* typedef Factory<ParamsSensorBase, std::string> FactoryParamsSensor;
* typedef Factory<ParamsProcessorBase, std::string> FactoryParamsProcessor;
* typedef Factory<LandmarkBase, YAML::Node> FactoryLandmark;
* typedef Factory<SensorBase,
* const std::string&,
* const Eigen::VectorXd&,
* const ParamsSensorBasePtr> FactorySensor;
* typedef Factory<LandmarkBasePtr, const YAML::Node&> FactoryLandmark;
* typedef Factory<StateBlockPtr, const Eigen::VectorXd&, bool> FactoryStateBlock;
*
* \endcode
*
* Second to know, the Factory class is a <a href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>:
* it can only exist once in your application.
* To access it, we internally use the static method get(),
* Second to know, the Factory class is a <a
* href="http://stackoverflow.com/questions/1008019/c-singleton-design-pattern#1008289">singleton</a>: it can only
* exist once in your application. To access it, we internally use the static method get(),
*
* \code
* Factory<MyTypeBase, MyTypeInput>::get()
......@@ -155,7 +117,8 @@ namespace wolf
*
* but this is a private method.
*
* The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via .get().
* The interesting methods are designed static, and are the ones responsible for accessing the Factory instance via
* .get().
*
* You must therefore call the methods you like statically, e.g. to create a landmark, you use:
*
......@@ -170,17 +133,15 @@ namespace wolf
* The API puts into play the two template parameters:
*
* \code
* static TypeBase* create( const TypeInput& );
* static std::shared_ptr<TypeBase> create( const TypeInput& );
* \endcode
*
* This API includes an element of type TypeInput, which might be either a std::string, or a YAML::node:
* - ````std::string```` is used to indicate the name of a configuration file. These files are usually YAML files containing configuration data to create your object.
* - ````YAML::Node```` is used to access parts of a YAML file already encoded as nodes, such as when loading landmarks from a SLAM map stored as a YAML file.
* This API includes elements defined as TypeInput, which might be one or more inputs.
*
* Two examples:
* For the two examples introduced before, the corresponding creators would be:
*
* \code
* static ParamsSensorBasePtr create(const std::string& _intrinsics_dot_yaml)
* static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed)
* static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node)
* \endcode
*
......@@ -203,15 +164,16 @@ namespace wolf
* For example, in sensor_camera_yaml.cpp we find the line:
*
* \code
* const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera", createParamsSensorCamera);
* \endcode
* const bool registered_camera_intr = FactoryParamsSensor::registerCreator("ParamsSensorCamera",
* createParamsSensorCamera); \endcode
*
* which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class).
*
* Therefore, at application level, all objects that have a .cpp file compiled are automatically registered.
*
* #### Unregister object creators
* The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of the object type.
* The method unregisterCreator() unregisters the ObjectXxx::create() method. It only needs to be passed the string of
* the object type.
*
* \code
* Factory<MyTypeBase, MyTypeInput>::unregisterCreator("ParamsSensorCamera");
......@@ -239,7 +201,7 @@ namespace wolf
* You can find this code in the landmark_polyline_2d.cpp file.
*
* \code
* // Creator (this method is static):
* // Creator (this method is static):
* LandmarkBasePtr LandmarkPolyline2d::create(const YAML::Node& _lmk_node)
* {
* // Parse YAML node with lmk info and data
......@@ -268,174 +230,159 @@ namespace wolf
*
* \code
* // Register landmark creator (put the register code inside an unnamed namespace):
* namespace
* {
* const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d", LandmarkPolyline2d::create);
* }
* namespace
* {
* const bool registered_lmk_polyline_2d = FactoryLandmark::registerCreator("LandmarkPolyline2d",
* LandmarkPolyline2d::create);
* }
*
* \endcode
*
* ### More information
* - FactoryParamsSensor: typedef of this template to create intrinsic structs deriving from ParamsSensorBase directly from YAML files.
* - FactoryParamsProcessor: typedef of this template to create processor params structs deriving from ParamsProcessorBase directly from YAML files.
* - FactoryLandmark: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes.
* - FactorySensorNode: typedef of this template to create Sensor classes deriving from SensorBase directly
* from YAML files.
* - FactoryProcessor: typedef of this template to create processor classes deriving from
* ProcessorBase directly from YAML files.
* - FactoryLandmark: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML
* nodes.
* - Problem::loadMap() : to load a maps directly from YAML files.
* - You can also check the code in the example file ````src/examples/test_map_yaml.cpp````.
*
* #### See also
* - FactorySensor: to create sensors
* - FactorySensorNode: to create sensors
* - FactoryProcessor: to create processors.
* - Problem::installSensor() : to install sensors in WOLF Problem.
* - Problem::installProcessor() : to install processors in WOLF Problem.
*
*/
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
class Factory
{
private:
typedef std::shared_ptr<TypeBase> TypeBasePtr;
public:
// example of creator callback (see typedefs below)
typedef TypeBase (*CreatorCallback)(TypeInput... _input);
private:
typedef std::map<std::string, CreatorCallback> CallbackMap;
public:
// example of creator callback (see typedefs below)
typedef TypeBasePtr (*CreatorCallback)(TypeInput... _input);
public:
static bool registerCreator(const std::string& _type, CreatorCallback createFn);
static bool unregisterCreator(const std::string& _type);
static bool isCreatorRegistered(const std::string& _type);
static TypeBase create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
static void printAddress();
static void printCallbacks();
static std::list<std::string> getRegisteredKeys();
private:
typedef std::map<std::string, CreatorCallback> CallbackMap;
private:
CallbackMap callbacks_;
public:
static bool registerCreator(const std::string& _type, CreatorCallback createFn);
static bool unregisterCreator(const std::string& _type);
static bool isCreatorRegistered(const std::string& _type);
static TypeBasePtr create(const std::string& _type, TypeInput... _input);
std::string getClass() const;
static void printAddress();
static void printCallbacks();
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
private:
static Factory& get();
private:
CallbackMap callbacks_;
public:
Factory(const Factory&) = delete;
void operator=(Factory const&) = delete;
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
private:
static Factory& get();
public:
Factory(const Factory&) = delete;
void operator=(Factory const&) = delete;
private:
Factory();
~Factory();
private:
Factory();
~Factory();
};
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>::Factory()
{
//std::cout << " Factory constructor " << this->getClass() << std::endl;
// std::cout << " Factory constructor " << this->getClass() << std::endl;
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>::~Factory()
{
//std::cout << " Factory destructor " << this->getClass() << std::endl;
// std::cout << " Factory destructor " << this->getClass() << std::endl;}
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::registerCreator(const std::string& _type, CreatorCallback createFn)
{
if (get().callbacks_.count(_type) == 1 and get().callbacks_.at(_type) != createFn)
throw std::runtime_error(get().getClass() + " : For the type \"" + _type +
"\": Trying to register a creator but there is a different creator registered.");
bool reg = get().callbacks_.insert(typename CallbackMap::value_type(_type, createFn)).second;
if (reg)
std::cout << std::setw(26) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
else
std::cout << std::setw(26) << std::left << get().getClass() << " X-- skipping " << _type << ": already registered." << std::endl;
if (reg) std::cout << std::setw(31) << std::left << get().getClass() << " <-- registered " << _type << std::endl;
return reg;
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::unregisterCreator(const std::string& _type)
{
return get().callbacks_.erase(_type) == 1;
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline bool Factory<TypeBase, TypeInput...>::isCreatorRegistered(const std::string& _type)
{
return get().callbacks_.count(_type) == 1;
}
template<class TypeBase, typename... TypeInput>
inline typename Factory<TypeBase, TypeInput...>::TypeBasePtr Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
template <class TypeBase, typename... TypeInput>
inline TypeBase Factory<TypeBase, TypeInput...>::create(const std::string& _type, TypeInput... _input)
{
typename CallbackMap::const_iterator creator_callback_it = get().callbacks_.find(_type);
if (creator_callback_it == get().callbacks_.end())
// not found
throw std::runtime_error(get().getClass() + " : Unknown type \"" + _type + "\". Possibly you tried to use an unregistered creator.");
throw std::runtime_error(get().getClass() + " : Unknown type \"" + _type +
"\". Possibly you tried to use an unregistered creator.");
// Invoke the creation function
return (creator_callback_it->second)(std::forward<TypeInput>(_input)...);
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline Factory<TypeBase, TypeInput...>& Factory<TypeBase, TypeInput...>::get()
{
static Factory<TypeBase, TypeInput...> instance_;
return instance_;
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline std::string Factory<TypeBase, TypeInput...>::getClass() const
{
return "Factory<class TypeBase>";
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline void Factory<TypeBase, TypeInput...>::printAddress()
{
std::cout << get().getClass() << " address: " << &get() << std::endl;
}
template<class TypeBase, typename... TypeInput>
template <class TypeBase, typename... TypeInput>
inline void Factory<TypeBase, TypeInput...>::printCallbacks()
{
std::cout << get().getClass() << " callbacks size: " << get().callbacks_.size() << std::endl;
for (auto cb: get().callbacks_){
for (auto cb : get().callbacks_)
{
std::cout << "\t" << cb.first << std::endl;
}
}
} // namespace wolf
namespace wolf
{
// Some specializations
//======================
// Landmarks from YAML
class LandmarkBase;
typedef Factory<LandmarkBase,
const YAML::Node&> FactoryLandmark;
template<>
inline std::string FactoryLandmark::getClass() const
template <class TypeBase, typename... TypeInput>
inline std::list<std::string> Factory<TypeBase, TypeInput...>::getRegisteredKeys()
{
return "FactoryLandmark";
std::list<std::string> keys;
for (auto cb : get().callbacks_)
{
keys.push_back(cb.first);
}
return keys;
}
#ifdef __GNUC__
#define WOLF_UNUSED __attribute__((used))
#elif defined _MSC_VER
#pragma warning(disable: Cxxxxx)
#define WOLF_UNUSED
#elif defined(__LCLINT__)
# define WOLF_UNUSED /*@unused@*/
#elif defined(__cplusplus)
# define WOLF_UNUSED
#else
# define UNUSED(x) x
#endif
} /* namespace wolf */
#endif /* FACTORY_H_ */
} // namespace wolf
//--------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--------
#ifndef NODE_BASE_H_
#define NODE_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
#include "core/utils/check_log.h"
namespace wolf {
namespace wolf
{
/** \brief Base class for Nodes
*
* Base class for all Nodes in the Wolf tree. Each node has
......@@ -48,10 +44,11 @@ namespace wolf {
* - "MAP"
* - "LANDMARK"
*
* - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples follow:
* - A unique type, which is a subcategory of the above. The list here cannot be exhaustive, but a few examples
* follow:
* - "SensorCamera"
* - "SensorLaser2d"
* - "LandmarkPoint3d"
* - "Landmark3d"
* - "ProcessorLaser2d"
*
* please refer to each base class derived from NodeLinked for better examples of their types.
......@@ -75,53 +72,56 @@ namespace wolf {
class NodeBase
{
friend Problem;
private:
static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
friend Problem;
struct Serializer;
private:
static unsigned int node_id_count_; ///< Object counter (acts as simple ID factory)
protected:
ProblemWPtr problem_ptr_;
struct Serializer;
unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc)
std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey", "Andrew", etc)
protected:
ProblemWPtr problem_ptr_;
bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
unsigned int node_id_; ///< Node id. It is unique over the whole Wolf Tree
std::string node_category_; ///< Text label identifying the category of node ("SENSOR", "FEATURE", etc)
std::string node_type_; ///< Text label identifying the type or subcategory of node ("Pin Hole", "Point 2d", etc)
std::string node_name_; ///< Text label identifying each specific object ("left camera", "LIDAR 1", "PointGrey",
///< "Andrew", etc)
virtual void setProblem(ProblemPtr _prob_ptr);
public:
bool is_removing_; ///< A flag for safely removing nodes from the Wolf tree. See remove().
NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
virtual ~NodeBase() = default;
virtual void setProblem(ProblemPtr _prob_ptr);
unsigned int nodeId() const;
std::string getCategory() const;
std::string getType() const;
std::string getName() const;
bool isRemoving() const;
public:
NodeBase(const std::string& _category, const std::string& _type = "Undefined", const std::string& _name = "");
virtual ~NodeBase() = default;
void setType(const std::string& _type);
void setName(const std::string& _name);
ProblemConstPtr getProblem() const;
ProblemPtr getProblem();
unsigned int nodeId() const;
std::string getCategory() const;
std::string getType() const;
std::string getName() const;
bool isRemoving() const;
virtual bool hasChildren() const = 0;
void setType(const std::string& _type);
void setName(const std::string& _name);
ProblemConstPtr getProblem() const;
ProblemPtr getProblem();
};
} // namespace wolf
} // namespace wolf
#include <iostream>
namespace wolf{
inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name) :
problem_ptr_(), // nullptr
node_id_(++node_id_count_),
node_category_(_category),
node_type_(_type),
node_name_(_name),
is_removing_(false)
namespace wolf
{
inline NodeBase::NodeBase(const std::string& _category, const std::string& _type, const std::string& _name)
: problem_ptr_(), // nullptr
node_id_(++node_id_count_),
node_category_(_category),
node_type_(_type),
node_name_(_name),
is_removing_(false)
{
//
}
......@@ -176,6 +176,4 @@ inline void NodeBase::setProblem(ProblemPtr _prob_ptr)
problem_ptr_ = _prob_ptr;
}
} // namespace wolf
#endif /* NODE_BASE_H_ */
} // namespace wolf
// 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/>.
#pragma once
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/composite/type_composite.h"
#include "core/composite/vector_composite.h"
#include "core/composite/prior_composite.h"
#include <map>
namespace wolf
{
class NodeStateBlocks : public NodeBase, public std::enable_shared_from_this<NodeStateBlocks>
{
friend Problem;
friend FactorBase;
public:
/** \brief Constructor
*
* \param _category TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: category name (see node_base.h)
* \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived type name
* \param _state_types Composite of types of the states that the node can have. Probably to be hardcoded in the
* derived class constructor.
* \param _state_vectors Composite of vectors of the states of the node. A 'StateBlock' for each key in
* '_state_vectors' will be emplaced (of the type specified in '_state_types'). Not all keys of '_state_types' are
* required to be specified, but all keys of '_state_vectors' should be in '_state_types'.
* \param _state_priors Composite of priors (initial_guess, factor or fixed) of the states of the node. Not all
* keys of '_state_vectors' are required to be specified, but all keys of '_state_priors' should be in
* '_state_vectors'. If not specified, no prior is assumed, i.e. 'initial_guess'.
*
**/
NodeStateBlocks(const std::string& _category,
const std::string& _type,
const TypeComposite& _state_types,
const VectorComposite& _state_vectors,
const PriorComposite& _state_priors);
virtual ~NodeStateBlocks();
virtual YAML::Node toYaml() const;
StateKeys getKeys() const;
TypeComposite getStateTypes() const;
bool has(const char& _sb_key) const;
bool has(const std::string& _keys) const;
virtual unsigned int id() const = 0;
std::map<char, StateBlockConstPtr> getStateBlockMap() const;
std::map<char, StateBlockPtr> getStateBlockMap();
std::vector<StateBlockConstPtr> getStateBlockVec() const;
std::vector<StateBlockPtr> getStateBlockVec();
// Some typical shortcuts -- not all should be coded here, see notes below.
StateBlockConstPtr getP() const;
StateBlockConstPtr getO() const;
StateBlockPtr getP();
StateBlockPtr getO();
// These act on all state blocks. Per-block action must be done through state_block.fix() or through extended API
// in derived classes of this.
virtual void fix();
virtual void unfix();
virtual bool isFixed() const;
/** \brief Emplace a state block
*
* \param _key Key of the new state.
* \param _type Types of the new state.
* \param _vector Vector of the new state.
* \param _fixed If the new state is fixed (i.e. not estimated).
**/
StateBlockPtr emplaceStateBlock(const char key,
const std::string& _type,
const Eigen::VectorXd& _vector,
const bool& _fixed);
/** \brief Emplace a state block with zero state
*
* \param _key Key of the new state.
* \param _type Types of the new state.
* \param _fixed If the new state is fixed (i.e. not estimated).
**/
StateBlockPtr emplaceStateBlockZero(const char key, const std::string& _type, const bool& _fixed);
protected:
virtual void removeStateBlock(const char& _sb_key);
public:
bool hasStateBlock(const StateBlockConstPtr& _sb) const;
bool setStateBlock(const char _sb_key, const StateBlockPtr& _sb);
bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
virtual StateBlockConstPtr getStateBlock(const char& _sb_key) const;
virtual StateBlockPtr getStateBlock(const char& _sb_key);
// Register/remove state blocks to/from wolf::Problem
void setProblem(ProblemPtr _problem) override;
virtual void remove(bool viral_remove_parent_without_children = false);
bool hasChildren() const override;
// Factors
private:
FactorBasePtr addFactor(FactorBasePtr _co_ptr);
void removeFactor(FactorBasePtr _co_ptr);
public:
/** \brief Add an absolute factor to a parameter
*
* Add an absolute factor to a parameter
* \param _i state block index (in state_block_vec_) of the parameter to be constrained
* \param _x prior value
* \param _cov covariance
* \param _start_idx state segment starting index (not used in quaternions)
* \param _size state segment size (-1: whole state) (not used in quaternions)
*
**/
void emplaceFactorStateBlock(const char& _key,
const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0);
void emplacePriors();
FactorBaseConstPtrSet getFactoredBySet() const;
FactorBasePtrSet getFactoredBySet();
void getFactoredBySet(FactorBaseConstPtrSet& _fac_set) const;
void getFactoredBySet(FactorBasePtrSet& _fac_set);
FactorBaseConstPtrSet getSensoryFactorSet() const;
FactorBasePtrSet getSensoryFactorSet();
std::map<char, FactorBaseConstPtr> getPriorFactorMap() const;
std::map<char, FactorBasePtr> getPriorFactorMap();
FactorBaseConstPtr getPriorFactor(char) const;
FactorBasePtr getPriorFactor(char);
FactorBaseConstPtrList getFactorsOf(ProcessorBaseConstPtr _processor_ptr, const std::string& type = "") const;
FactorBasePtrList getFactorsOf(ProcessorBasePtr _processor_ptr, const std::string& type = "");
bool hasFactor(FactorBaseConstPtr _fac) const;
// States
VectorComposite getState(const StateKeys& _keys = "") const;
void setState(const VectorComposite& _state, const bool _notify = true);
void setState(const Eigen::VectorXd& _state,
const StateKeys& _keys,
const std::list<SizeEigen>& _sizes,
const bool _notify = true);
void setState(const Eigen::VectorXd& _state, const StateKeys& _keys, const bool _notify = true);
void setState(const StateKeys& _keys, const std::list<Eigen::VectorXd>& _vectors, const bool _notify = true);
Eigen::VectorXd getStateVector(const StateKeys& _keys) const;
unsigned int getSize(const StateKeys& _keys = "") const;
unsigned int getLocalSize(const StateKeys& _keys = "") const;
// Covariance
bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const;
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.01);
// Derived type checks
bool isCapture() const;
bool isFrame() const;
bool isLandmark() const;
bool isSensor() const;
protected:
// transform state
void transform(const VectorComposite& _transformation);
void printState(bool _factored_by,
bool _metric,
bool _state_blocks,
std::ostream& _stream,
std::string _tabs) const;
CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs) const;
private:
Composite<StateBlockPtr> state_blocks_;
PriorComposite state_priors_;
TypeComposite state_types_;
FactorBasePtrSet factored_by_set_;
FactorBasePtrSet factor_sensory_set_;
std::map<char, FactorBasePtr> factor_prior_map_;
};
} // namespace wolf
//////////// IMPLEMENTATION /////////////
#include "core/state_block/state_block.h"
namespace wolf
{
inline NodeStateBlocks::~NodeStateBlocks()
{
//
}
inline StateKeys NodeStateBlocks::getKeys() const
{
return state_blocks_.getKeys();
}
inline TypeComposite NodeStateBlocks::getStateTypes() const
{
return state_types_;
}
inline bool NodeStateBlocks::has(const char& _sb_key) const
{
return state_blocks_.has(_sb_key);
}
inline bool NodeStateBlocks::has(const std::string& _keys) const
{
return state_blocks_.has(_keys);
}
inline std::map<char, StateBlockConstPtr> NodeStateBlocks::getStateBlockMap() const
{
std::map<char, StateBlockConstPtr> map_const;
for (auto&& pair : state_blocks_) map_const[pair.first] = pair.second;
return map_const;
}
inline std::map<char, StateBlockPtr> NodeStateBlocks::getStateBlockMap()
{
return state_blocks_;
}
inline std::vector<StateBlockConstPtr> NodeStateBlocks::getStateBlockVec() const
{
std::vector<StateBlockConstPtr> sbv;
for (auto key : getKeys()) sbv.push_back(getStateBlock(key));
return sbv;
}
inline std::vector<StateBlockPtr> NodeStateBlocks::getStateBlockVec()
{
std::vector<StateBlockPtr> sbv;
for (auto key : getKeys()) sbv.push_back(getStateBlock(key));
return sbv;
}
inline bool NodeStateBlocks::setStateBlock(const char _sb_key, const StateBlockPtr& _sb)
{
assert(state_blocks_.count(_sb_key) > 0 and "Cannot set an inexistent state block! Use addStateBlock instead.");
WOLF_WARN_COND(
factor_prior_map_.count(_sb_key),
"NodeStateBlocks::setStateBlock: this state block has an absolute factor, setting state but solver may "
"change it again...");
state_blocks_.at(_sb_key) = _sb;
return true; // success
}
inline wolf::StateBlockConstPtr NodeStateBlocks::getStateBlock(const char& _sb_key) const
{
auto it = state_blocks_.find(_sb_key);
return it != state_blocks_.end() ? it->second : nullptr;
}
inline wolf::StateBlockPtr NodeStateBlocks::getStateBlock(const char& _sb_key)
{
auto it = state_blocks_.find(_sb_key);
return it != state_blocks_.end() ? it->second : nullptr;
}
inline wolf::StateBlockConstPtr NodeStateBlocks::getP() const
{
return getStateBlock('P');
}
inline wolf::StateBlockPtr NodeStateBlocks::getP()
{
return getStateBlock('P');
}
inline wolf::StateBlockConstPtr NodeStateBlocks::getO() const
{
return getStateBlock('O');
}
inline wolf::StateBlockPtr NodeStateBlocks::getO()
{
return getStateBlock('O');
}
inline void NodeStateBlocks::fix()
{
for (auto pair_key_sbp : state_blocks_)
if (pair_key_sbp.second != nullptr) pair_key_sbp.second->fix();
}
inline void NodeStateBlocks::unfix()
{
for (auto pair_key_sbp : state_blocks_)
if (pair_key_sbp.second != nullptr) pair_key_sbp.second->unfix();
}
inline FactorBaseConstPtrSet NodeStateBlocks::getSensoryFactorSet() const
{
FactorBaseConstPtrSet set_const;
for (auto&& obj_ptr : factor_sensory_set_) set_const.insert(obj_ptr);
return set_const;
}
inline FactorBasePtrSet NodeStateBlocks::getSensoryFactorSet()
{
return factor_sensory_set_;
}
inline std::map<char, FactorBaseConstPtr> NodeStateBlocks::getPriorFactorMap() const
{
std::map<char, FactorBaseConstPtr> map_const;
for (auto&& obj_it : factor_prior_map_) map_const[obj_it.first] = obj_it.second;
return map_const;
}
inline std::map<char, FactorBasePtr> NodeStateBlocks::getPriorFactorMap()
{
return factor_prior_map_;
}
inline FactorBaseConstPtrSet NodeStateBlocks::getFactoredBySet() const
{
FactorBaseConstPtrSet set_const;
for (auto&& obj_ptr : factored_by_set_) set_const.insert(obj_ptr);
return set_const;
}
inline FactorBasePtrSet NodeStateBlocks::getFactoredBySet()
{
return factored_by_set_;
}
inline void NodeStateBlocks::getFactoredBySet(FactorBaseConstPtrSet& _fac_set) const
{
_fac_set.insert(factored_by_set_.begin(), factored_by_set_.end()); // std insert takes care of duplicated elements
}
inline void NodeStateBlocks::getFactoredBySet(FactorBasePtrSet& _fac_set)
{
_fac_set.insert(factored_by_set_.begin(), factored_by_set_.end()); // std insert takes care of duplicated elements
}
inline FactorBaseConstPtr NodeStateBlocks::getPriorFactor(char key) const
{
if (factor_prior_map_.count(key) == 0) return nullptr;
return factor_prior_map_.at(key);
}
inline FactorBasePtr NodeStateBlocks::getPriorFactor(char key)
{
if (factor_prior_map_.count(key) == 0) return nullptr;
return factor_prior_map_.at(key);
}
inline bool NodeStateBlocks::hasFactor(FactorBaseConstPtr _factor) const
{
return getFactoredBySet().count(_factor);
}
inline bool NodeStateBlocks::hasChildren() const
{
return not factored_by_set_.empty();
}
inline bool NodeStateBlocks::isCapture() const
{
return std::dynamic_pointer_cast<const CaptureBase>(shared_from_this()) != nullptr;
}
inline bool NodeStateBlocks::isFrame() const
{
return std::dynamic_pointer_cast<const FrameBase>(shared_from_this()) != nullptr;
}
inline bool NodeStateBlocks::isLandmark() const
{
return std::dynamic_pointer_cast<const LandmarkBase>(shared_from_this()) != nullptr;
}
inline bool NodeStateBlocks::isSensor() const
{
return std::dynamic_pointer_cast<const SensorBase>(shared_from_this()) != nullptr;
}
} // namespace wolf
//--------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--------
#ifndef PARAMS_BASE_H_
#define PARAMS_BASE_H_
#include "core/utils/params_server.h"
namespace wolf {
struct ParamsBase
{
ParamsBase() = default;
ParamsBase(std::string _unique_name, const ParamsServer&)
{
//
}
virtual ~ParamsBase() = default;
virtual std::string print() const = 0;
};
}
#endif
// 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/>.
#pragma once
#include <iostream>
#include <iomanip>
#include <chrono>
#include <cmath>
namespace wolf
{
/**
* @class ProfilingUnit
* @brief A class for profiling code execution time and statistics.
*
* The ProfilingUnit class provides methods to start and stop profiling, and to
* print profiling results.
*/
class ProfilingUnit
{
protected:
bool running_; ///< True if profiling is running.
double desired_period_; ///< Desired period in microseconds (< 0 for not specified)
unsigned int n_callbacks_; ///< Number of callbacks.
double average_period_; ///< Average period in microseconds.
std::chrono::microseconds acc_duration_; ///< Accumulated duration in microseconds.
std::chrono::microseconds max_duration_; ///< Maximum duration in microseconds.
double var_duration_; ///< Variance of the duration in microseconds squared.
std::chrono::time_point<std::chrono::high_resolution_clock> first_clock_; ///< Time point of the first clock.
std::chrono::time_point<std::chrono::high_resolution_clock> start_clock_; ///< Time point of the start clock.
public:
/**
* @brief Constructor
* @param desired_period_sec Desired period in seconds. Negative value means not specified. Default is -1.
*/
ProfilingUnit(double desired_period_sec = -1);
/**
* @brief Start profiling.
*/
void startProfiling();
/**
* @brief Stop profiling.
*/
void stopProfiling();
/**
* @brief Print profiling results.
* @param _tabs Tabs to be used for indentation.
* @param stream Output stream to print the results. Default is std::cout.
*/
void printProfiling(const std::string& _tabs, std::ostream& stream = std::cout) const;
/**
* @brief Print provided duration (in us) in the most appropiate units with specified precision.
* @param _duration_microsec Duration in microseconds.
* @param precision Precision for printing the duration. Default is 2.
* @return A string representing the duration.
*/
std::string printDuration(double _duration_microsec, int precision = 2) const;
/**
* @brief Set the desired period.
* @param _desired_period_sec Desired period in seconds.
*/
void setDesiredPeriod(double _desired_period_sec);
/**
* @brief Check if profiling is running.
* @return True if profiling is running, false otherwise.
*/
bool isRunning() const;
/**
* @brief Get the number of callbacks.
* @return Number of callbacks.
*/
unsigned int getNCallbacks() const;
/**
* @brief Get the desired period.
* @return Desired period in seconds.
*/
double getDesiredPeriod() const;
/**
* @brief Get the average period.
* @return Average period in seconds.
*/
double getAveragePeriod() const;
/**
* @brief Get the accumulated duration.
* @return Accumulated duration in seconds.
*/
double getAccDuration() const;
/**
* @brief Get the average duration.
* @return Average duration in seconds.
*/
double getAvgDuration() const;
/**
* @brief Get the maximum duration.
* @return Maximum duration in seconds.
*/
double getMaxDuration() const;
/**
* @brief Get the variance of the duration.
* @return Variance of the duration in seconds squared.
*/
double getVarDuration() const;
/**
* @brief Get the standard deviation of the duration.
* @return Standard deviation of the duration in seconds.
*/
double getStDevDuration() const;
};
inline bool ProfilingUnit::isRunning() const
{
return running_;
}
inline unsigned int ProfilingUnit::getNCallbacks() const
{
return n_callbacks_;
}
inline double ProfilingUnit::getDesiredPeriod() const
{
return 1e-6 * desired_period_;
}
inline double ProfilingUnit::getAveragePeriod() const
{
return 1e-6 * average_period_;
}
inline double ProfilingUnit::getAccDuration() const
{
return 1e-6 * acc_duration_.count();
}
inline double ProfilingUnit::getAvgDuration() const
{
return 1e-6 * acc_duration_.count() / n_callbacks_;
}
inline double ProfilingUnit::getMaxDuration() const
{
return 1e-6 * max_duration_.count();
}
inline double ProfilingUnit::getVarDuration() const
{
return 1e-12 * var_duration_;
}
inline double ProfilingUnit::getStDevDuration() const
{
return 1e-6 * sqrt(var_duration_);
}
} // namespace wolf
//--------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/>.
#pragma once
#ifndef TIME_STAMP_H_
#define TIME_STAMP_H_
//wolf includes
// wolf includes
#include "core/common/wolf.h"
//C, std
// C, std
#include <sys/time.h>
#include <iostream>
#include <iomanip>
static const unsigned int NANOSECS = 1000000000;
namespace wolf {
namespace wolf
{
/**
* \brief TimeStamp implements basic functionalities for time stamps
*
*
* TimeStamp implements basic functionalities for time stamps
*/
class TimeStamp
{
protected:
unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
bool is_valid_; // time stamp has a valid value
public:
/** \brief Constructor
*
* Constructor without arguments. Sets time stamp to now.
*
*/
TimeStamp();
/** \brief Copy constructor
*
* Copy constructor
*
*/
TimeStamp(const TimeStamp& _ts);
/** \brief Constructor with argument
*
* Constructor with arguments
*
*/
TimeStamp(const double& _ts);
/** \brief Constructor from sec and nsec
*
* Constructor from sec and nsec
*
*/
TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
/** \brief Destructor
*
* Destructor
*
*/
~TimeStamp();
/** \brief Value of time stamp is valid
*
*/
static TimeStamp Invalid ( );
bool ok ( ) const;
void setOk ( );
void setNOk ( );
static TimeStamp Now();
/** \brief Time stamp to now
*/
void setToNow();
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a timeval struct
*/
void set(const timeval& ts);
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
*
*/
void set(const unsigned long int& sec, const unsigned long int& nanosec);
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a scalar_t (seconds)
*
*/
void set(const double& ts);
/** \brief Get time stamp
*
* Returns time stamp
*
*/
double get() const;
/** \brief Get time stamp (only seconds)
*
* Returns seconds of time stamp
*
*/
unsigned long int getSeconds() const;
/** \brief Get time stamp (only nano seconds)
*
* Returns nanoseconds part of time stamp
*
*/
unsigned long int getNanoSeconds() const;
/** \brief Assignement operator
*
* Assignement operator
*
*/
void operator =(const double& ts);
/** \brief Assignement operator
*
* Assignement operator given a scalar_t (seconds)
*
*/
void operator =(const TimeStamp& ts);
/** \brief comparison operator
*
* Comparison operator
*
*/
bool operator !=(const TimeStamp& ts) const;
bool operator ==(const TimeStamp& ts) const;
bool operator <(const TimeStamp& ts) const;
bool operator >(const TimeStamp& ts) const;
/** \brief comparison operator
*
* Comparison operator
*
*/
bool operator <=(const TimeStamp& ts) const;
bool operator >=(const TimeStamp& ts) const;
/** \brief Add-assign operator given a scalar_t (seconds)
*/
void operator +=(const double& dt);
/** \brief Add-assign operator given a scalar_t (seconds)
*/
TimeStamp operator +(const double& dt) const;
/** \brief Substraction-assign operator given a scalar_t (seconds)
*/
void operator -=(const double& dt);
/** \brief difference operator
*
* difference operator that returns a scalar_t (seconds)
*
*/
TimeStamp operator -(const double& ts) const;
/** \brief difference operator
*
* difference operator that returns a Timestamp (seconds)
*
*/
double operator -(const TimeStamp& ts) const;
/** \brief Prints time stamp to a given ostream
*
* Prints time stamp to a given ostream
*
*/
void print(std::ostream & ost = std::cout) const;
friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts);
protected:
unsigned long int time_stamp_nano_; ///< Time stamp. Expressed in nanoseconds from 1th jan 1970.
bool is_valid_; // time stamp has a valid value
public:
/** \brief Constructor
*
* Constructor without arguments. Sets time stamp to now.
*
*/
TimeStamp();
/** \brief Copy constructor
*
* Copy constructor
*
*/
TimeStamp(const TimeStamp& _ts);
/** \brief Constructor with argument
*
* Constructor with arguments
*
*/
TimeStamp(const double& _ts);
/** \brief Constructor from sec and nsec
*
* Constructor from sec and nsec
*
*/
TimeStamp(const unsigned long int& _sec, const unsigned long int& _nsec);
/** \brief Destructor
*
* Destructor
*
*/
~TimeStamp();
/** \brief Value of time stamp is valid
*
*/
static TimeStamp Invalid();
bool ok() const;
void setOk();
void setNOk();
static TimeStamp Now();
/** \brief Time stamp to now
*/
void setToNow();
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a timeval struct
*/
void set(const timeval& ts);
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a two-integer (seconds and nanoseconds)
*
*/
void set(const unsigned long int& sec, const unsigned long int& nanosec);
/** \brief Set time stamp
*
* Sets time stamp to a given value passed as a scalar_t (seconds)
*
*/
void set(const double& ts);
/** \brief Get time stamp
*
* Returns time stamp
*
*/
double get() const;
/** \brief Get time stamp (only seconds)
*
* Returns seconds of time stamp
*
*/
unsigned long int getSeconds() const;
/** \brief Get time stamp (only nano seconds)
*
* Returns nanoseconds part of time stamp
*
*/
unsigned long int getNanoSeconds() const;
/** \brief Assignement operator
*
* Assignement operator
*
*/
void operator=(const double& ts);
/** \brief Assignement operator
*
* Assignement operator given a scalar_t (seconds)
*
*/
void operator=(const TimeStamp& ts);
/** \brief comparison operator
*
* Comparison operator
*
*/
bool operator!=(const TimeStamp& ts) const;
bool operator==(const TimeStamp& ts) const;
bool operator<(const TimeStamp& ts) const;
bool operator>(const TimeStamp& ts) const;
/** \brief comparison operator
*
* Comparison operator
*
*/
bool operator<=(const TimeStamp& ts) const;
bool operator>=(const TimeStamp& ts) const;
/** \brief Add-assign operator given a scalar_t (seconds)
*/
void operator+=(const double& dt);
/** \brief Add-assign operator given a scalar_t (seconds)
*/
TimeStamp operator+(const double& dt) const;
/** \brief Substraction-assign operator given a scalar_t (seconds)
*/
void operator-=(const double& dt);
/** \brief difference operator
*
* difference operator that returns a scalar_t (seconds)
*
*/
TimeStamp operator-(const double& ts) const;
/** \brief difference operator
*
* difference operator that returns a Timestamp (seconds)
*
*/
double operator-(const TimeStamp& ts) const;
/** \brief Prints time stamp to a given ostream
*
* Prints time stamp to a given ostream
*
*/
void print(std::ostream& ost = std::cout) const;
friend std::ostream& operator<<(std::ostream& os, const TimeStamp& _ts);
};
inline wolf::TimeStamp TimeStamp::Invalid ( )
inline wolf::TimeStamp TimeStamp::Invalid()
{
return TimeStamp(-1.0);
}
inline bool TimeStamp::ok ( ) const
inline bool TimeStamp::ok() const
{
return is_valid_;
}
inline void TimeStamp::setOk ( )
inline void TimeStamp::setOk()
{
is_valid_ = true;
}
inline void TimeStamp::setNOk ( )
inline void TimeStamp::setNOk()
{
is_valid_ = false;
}
inline void TimeStamp::set(const double& ts)
{
time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts*NANOSECS) : 0);
is_valid_ = (ts >= 0);
time_stamp_nano_ = (ts >= 0 ? (unsigned long int)(ts * NANOSECS) : 0);
is_valid_ = (ts >= 0);
}
inline void TimeStamp::set(const unsigned long int& sec, const unsigned long int& nanosec)
{
time_stamp_nano_ = sec*NANOSECS+nanosec;
is_valid_ = true;
time_stamp_nano_ = sec * NANOSECS + nanosec;
is_valid_ = true;
}
inline void TimeStamp::set(const timeval& ts)
{
time_stamp_nano_ = (unsigned long int)(ts.tv_sec*NANOSECS) + (unsigned long int)(ts.tv_usec*1000);
is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0);
time_stamp_nano_ = (unsigned long int)(ts.tv_sec * NANOSECS) + (unsigned long int)(ts.tv_usec * 1000);
is_valid_ = (ts.tv_sec >= 0 and ts.tv_usec >= 0);
}
inline double TimeStamp::get() const
{
return ((double)( time_stamp_nano_))*1e-9;
return ((double)(time_stamp_nano_)) * 1e-9;
}
inline unsigned long int TimeStamp::getSeconds() const
......@@ -259,58 +253,57 @@ inline unsigned long int TimeStamp::getNanoSeconds() const
return time_stamp_nano_ % NANOSECS;
}
inline void TimeStamp::operator =(const TimeStamp& ts)
inline void TimeStamp::operator=(const TimeStamp& ts)
{
time_stamp_nano_ = ts.time_stamp_nano_;
is_valid_ = ts.is_valid_;
is_valid_ = ts.is_valid_;
}
inline void TimeStamp::operator =(const double& ts)
inline void TimeStamp::operator=(const double& ts)
{
time_stamp_nano_ = (unsigned long int)(ts*NANOSECS);
is_valid_ = (ts >= 0);
time_stamp_nano_ = (unsigned long int)(ts * NANOSECS);
is_valid_ = (ts >= 0);
}
inline bool TimeStamp::operator ==(const TimeStamp& ts) const
inline bool TimeStamp::operator==(const TimeStamp& ts) const
{
return (time_stamp_nano_ == ts.time_stamp_nano_);
}
inline bool TimeStamp::operator !=(const TimeStamp& ts) const
inline bool TimeStamp::operator!=(const TimeStamp& ts) const
{
return (time_stamp_nano_ != ts.time_stamp_nano_);
}
inline bool TimeStamp::operator <(const TimeStamp& ts) const
inline bool TimeStamp::operator<(const TimeStamp& ts) const
{
return (time_stamp_nano_ < ts.time_stamp_nano_);
}
inline bool TimeStamp::operator >(const TimeStamp& ts) const
inline bool TimeStamp::operator>(const TimeStamp& ts) const
{
return (time_stamp_nano_ > ts.time_stamp_nano_);
}
inline bool TimeStamp::operator <=(const TimeStamp& ts) const
inline bool TimeStamp::operator<=(const TimeStamp& ts) const
{
return (time_stamp_nano_ <= ts.time_stamp_nano_);
}
inline bool TimeStamp::operator >=(const TimeStamp& ts) const
inline bool TimeStamp::operator>=(const TimeStamp& ts) const
{
return (time_stamp_nano_ >= ts.time_stamp_nano_);
}
inline void TimeStamp::operator +=(const double& dt)
inline void TimeStamp::operator+=(const double& dt)
{
time_stamp_nano_ += (unsigned long int)(dt*NANOSECS);
time_stamp_nano_ += (unsigned long int)(dt * NANOSECS);
}
inline double TimeStamp::operator -(const TimeStamp& ts) const
inline double TimeStamp::operator-(const TimeStamp& ts) const
{
return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_))*1e-9; // long int cast fix overflow in case of negative substraction result
return double((long int)(time_stamp_nano_ - ts.time_stamp_nano_)) *
1e-9; // long int cast fix overflow in case of negative substraction result
}
} // namespace wolf
#endif
} // namespace wolf
//--------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--------
/**
* \file wolf.h
* \brief General typedefs for the Wolf project
* \author Joan Sola
* Created on: 28/05/2014
*/
#ifndef WOLF_H_
#define WOLF_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Enable project-specific definitions and macros
#include "core/internal/config.h"
#include "core/utils/load_core.h"
#include "core/utils/logging.h"
#include "core/utils/string_utils.h"
// Folder Registry
#include "core/utils/folder_registry.h"
//includes from Eigen lib
// includes from Eigen lib
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Sparse>
#include <libgen.h>
//includes from std lib
// includes from std lib
#include <list>
#include <map>
#include <memory> // shared_ptr and weak_ptr
#include <memory> // shared_ptr and weak_ptr
#include <set>
// System specifics
#include <sys/stat.h>
namespace wolf {
// YAML
#include "yaml-cpp/yaml.h"
#include "yaml-schema-cpp/yaml_conversion.hpp"
namespace wolf
{
/**
* \brief Vector and Matrices size type for the Wolf project
*
......@@ -70,15 +68,15 @@ typedef std::size_t SizeStd;
#define M_TORAD 0.017453292519943295769236907684886127134 // pi / 180
#define M_TODEG 57.295779513082320876798154814105170332 // 180 / pi
namespace Constants{
namespace Constants
{
// Wolf standard tolerance
const double EPS = 1e-8;
// Wolf smmmmall tolerance
const double EPS_SMALL = 1e-16;
}
} // namespace Constants
} // namespace wolf
} // namespace wolf
///////////////////////////////////////////
// Construct types for any scalar defined in the typedef double above
......@@ -95,30 +93,30 @@ const double EPS_SMALL = 1e-16;
namespace Eigen // Eigen namespace extension
{
// 1. Vectors and Matrices
typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type
typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type
typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type
typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type
typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type
typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type
typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type
typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
typedef Matrix<double, 1, 1> Matrix1d; ///< 1x1 matrix of real double type
typedef Matrix<double, 5, 5> Matrix5d; ///< 5x5 matrix of real double type
typedef Matrix<double, 6, 6> Matrix6d; ///< 6x6 matrix of real double type
typedef Matrix<double, 7, 7> Matrix7d; ///< 7x7 matrix of real double type
typedef Matrix<double, 8, 8> Matrix8d; ///< 8x8 matrix of real double type
typedef Matrix<double, 9, 9> Matrix9d; ///< 9x9 matrix of real double type
typedef Matrix<double, 10, 10> Matrix10d; ///< 10x10 matrix of real double type
typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
// 2. Sparse matrix
typedef SparseMatrix<double> SparseMatrixd;
// 3. Row major matrix
typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type
}
namespace wolf {
typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixRowXd; ///< dynamic rowmajor matrix of real double type
} // namespace Eigen
namespace wolf
{
//////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically
*
......@@ -127,7 +125,8 @@ namespace wolf {
* The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's
* StaticAssert.h):
*
* EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
* EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
......@@ -135,7 +134,8 @@ namespace wolf {
*
* but they do not work if the evaluated types are of dynamic size.
*
* In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
* In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this
* kind:
*
* template<typename Derived>
* inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
......@@ -152,44 +152,45 @@ namespace wolf {
* The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
* This check is performed statically or dynamically, depending on the type of argument provided.
*/
template<int Size, int DesiredSize>
template <int Size, int DesiredSize>
struct StaticSizeCheck
{
template<typename T>
StaticSizeCheck(const T&)
{
static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
}
template <typename T>
StaticSizeCheck(const T&)
{
static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
}
};
//
template<int DesiredSize>
template <int DesiredSize>
struct StaticSizeCheck<Eigen::Dynamic, DesiredSize>
{
template<typename T>
StaticSizeCheck(const T& t)
{
assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
}
template <typename T>
StaticSizeCheck(const T& t)
{
assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
}
};
//
template<int DesiredR, int DesiredC>
template <int DesiredR, int DesiredC>
struct MatrixSizeCheck
{
/** \brief Assert matrix size dynamically or statically
*
* @param t the variable for which we wish to assert the size.
*
* Usage: to assert that t is size (Rows x Cols)
*
* MatrixSizeCheck<Rows, Cols>::check(t);
*/
template<typename T>
static void check(const T& t)
{
StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
}
/** \brief Assert matrix size dynamically or statically
*
* @param t the variable for which we wish to assert the size.
*
* Usage: to assert that t is size (Rows x Cols)
*
* MatrixSizeCheck<Rows, Cols>::check(t);
*/
template <typename T>
static void check(const T& t)
{
StaticSizeCheck<T::RowsAtCompileTime, DesiredR>(t.rows());
StaticSizeCheck<T::ColsAtCompileTime, DesiredC>(t.cols());
}
};
//
// End of check matrix sizes /////////////////////////////////////////////////
......@@ -197,107 +198,113 @@ struct MatrixSizeCheck
// TYPEDEFS FOR POINTERS, LISTS AND ITERATORS IN THE WOLF TREE
/////////////////////////////////////////////////////////////////////////
#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
typedef std::shared_ptr<Name> Name##Ptr; \
typedef std::shared_ptr<const Name> Name##ConstPtr; \
typedef std::weak_ptr<Name> Name##WPtr; \
typedef std::weak_ptr<const Name> Name##ConstWPtr; \
#define WOLF_PTR_TYPEDEFS(ClassName) \
class ClassName; \
WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
WOLF_DECLARED_PTR_TYPEDEFS(StructName); \
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
typedef std::list<ClassName##Ptr> ClassName##PtrList; \
typedef ClassName##PtrList::iterator ClassName##PtrListIter; \
typedef ClassName##PtrList::const_iterator ClassName##PtrListConstIter; \
typedef ClassName##PtrList::reverse_iterator ClassName##PtrListRevIter; \
typedef std::list<ClassName##WPtr> ClassName##WPtrList; \
typedef ClassName##WPtrList::iterator ClassName##WPtrListIter; \
typedef ClassName##WPtrList::const_iterator ClassName##WPtrListConstIter; \
typedef ClassName##WPtrList::reverse_iterator ClassName##WPtrListRevIter; \
typedef std::list<ClassName##ConstPtr> ClassName##ConstPtrList; \
typedef ClassName##ConstPtrList::iterator ClassName##ConstPtrListIter; \
typedef ClassName##ConstPtrList::const_iterator ClassName##ConstPtrListConstIter; \
typedef ClassName##ConstPtrList::reverse_iterator ClassName##ConstPtrListRevIter; \
typedef std::list<ClassName##ConstWPtr> ClassName##ConstWPtrList; \
typedef ClassName##ConstWPtrList::iterator ClassName##ConstWPtrListIter; \
typedef ClassName##ConstWPtrList::const_iterator ClassName##ConstWPtrListConstIter; \
typedef ClassName##ConstWPtrList::reverse_iterator ClassName##ConstWPtrListRevIter; \
#define WOLF_DECLARED_PTR_TYPEDEFS(Name) \
typedef std::shared_ptr<Name> Name##Ptr; \
typedef std::shared_ptr<const Name> Name##ConstPtr; \
typedef std::weak_ptr<Name> Name##WPtr; \
typedef std::weak_ptr<const Name> Name##ConstWPtr;
#define WOLF_DECLARED_LIST_TYPEDEFS(Name) \
typedef std::list<Name##Ptr> Name##PtrList; \
typedef Name##PtrList::iterator Name##PtrListIter; \
typedef Name##PtrList::const_iterator Name##PtrListConstIter; \
typedef Name##PtrList::reverse_iterator Name##PtrListRevIter; \
typedef std::list<Name##WPtr> Name##WPtrList; \
typedef Name##WPtrList::iterator Name##WPtrListIter; \
typedef Name##WPtrList::const_iterator Name##WPtrListConstIter; \
typedef Name##WPtrList::reverse_iterator Name##WPtrListRevIter; \
typedef std::list<Name##ConstPtr> Name##ConstPtrList; \
typedef Name##ConstPtrList::iterator Name##ConstPtrListIter; \
typedef Name##ConstPtrList::const_iterator Name##ConstPtrListConstIter; \
typedef Name##ConstPtrList::reverse_iterator Name##ConstPtrListRevIter; \
typedef std::list<Name##ConstWPtr> Name##ConstWPtrList; \
typedef Name##ConstWPtrList::iterator Name##ConstWPtrListIter; \
typedef Name##ConstWPtrList::const_iterator Name##ConstWPtrListConstIter; \
typedef Name##ConstWPtrList::reverse_iterator Name##ConstWPtrListRevIter;
#define WOLF_DECLARED_SET_TYPEDEFS(Name) \
typedef std::set<Name##Ptr> Name##PtrSet; \
typedef Name##PtrSet::iterator Name##PtrSetIter; \
typedef Name##PtrSet::const_iterator Name##PtrSetConstIter; \
typedef Name##PtrSet::reverse_iterator Name##PtrSetRevIter; \
typedef std::set<Name##WPtr> Name##WPtrSet; \
typedef Name##WPtrSet::iterator Name##WPtrSetIter; \
typedef Name##WPtrSet::const_iterator Name##WPtrSetConstIter; \
typedef Name##WPtrSet::reverse_iterator Name##WPtrSetRevIter; \
typedef std::set<Name##ConstPtr> Name##ConstPtrSet; \
typedef Name##ConstPtrSet::iterator Name##ConstPtrSetIter; \
typedef Name##ConstPtrSet::const_iterator Name##ConstPtrSetConstIter; \
typedef Name##ConstPtrSet::reverse_iterator Name##ConstPtrSetRevIter; \
typedef std::set<Name##ConstWPtr> Name##ConstWPtrSet; \
typedef Name##ConstWPtrSet::iterator Name##ConstWPtrSetIter; \
typedef Name##ConstWPtrSet::const_iterator Name##ConstWPtrSetConstIter; \
typedef Name##ConstWPtrSet::reverse_iterator Name##ConstWPtrSetRevIter;
#define WOLF_PTR_TYPEDEFS(ClassName) \
class ClassName; \
WOLF_DECLARED_PTR_TYPEDEFS(ClassName); \
WOLF_DECLARED_LIST_TYPEDEFS(ClassName); \
WOLF_DECLARED_SET_TYPEDEFS(ClassName);
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
WOLF_DECLARED_PTR_TYPEDEFS(StructName); \
WOLF_DECLARED_LIST_TYPEDEFS(StructName); \
WOLF_DECLARED_SET_TYPEDEFS(StructName);
// NodeBase
WOLF_PTR_TYPEDEFS(NodeBase);
// NodeStateBlocks
WOLF_PTR_TYPEDEFS(NodeStateBlocks);
// Problem
WOLF_PTR_TYPEDEFS(Problem);
// Tree Manager
WOLF_PTR_TYPEDEFS(TreeManagerBase);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerBase);
// Hardware
WOLF_PTR_TYPEDEFS(HardwareBase);
// - Sensors
WOLF_PTR_TYPEDEFS(SensorBase);
WOLF_LIST_TYPEDEFS(SensorBase);
// - - ParamsSensor
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase);
// - Processors
WOLF_PTR_TYPEDEFS(ProcessorBase);
WOLF_LIST_TYPEDEFS(ProcessorBase);
// - ProcessorMotion
WOLF_PTR_TYPEDEFS(ProcessorMotion);
// - - Processor params
WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorBase);
// Trajectory
WOLF_PTR_TYPEDEFS(TrajectoryBase);
// - Frame
WOLF_PTR_TYPEDEFS(FrameBase);
WOLF_LIST_TYPEDEFS(FrameBase);
class TimeStamp;
typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
typedef std::map<TimeStamp, FrameBasePtr> FramePtrMap;
typedef std::map<TimeStamp, FrameBaseConstPtr> FrameConstPtrMap;
// - Capture
WOLF_PTR_TYPEDEFS(CaptureBase);
WOLF_LIST_TYPEDEFS(CaptureBase);
// - Feature
WOLF_PTR_TYPEDEFS(FeatureBase);
WOLF_LIST_TYPEDEFS(FeatureBase);
// - Factor
WOLF_PTR_TYPEDEFS(FactorBase);
WOLF_LIST_TYPEDEFS(FactorBase);
// Map
WOLF_PTR_TYPEDEFS(MapBase);
// - - Map params
WOLF_STRUCT_PTR_TYPEDEFS(ParamsMapBase);
// - Landmark
WOLF_PTR_TYPEDEFS(LandmarkBase);
WOLF_LIST_TYPEDEFS(LandmarkBase);
// - - State blocks
WOLF_PTR_TYPEDEFS(StateBlock);
WOLF_LIST_TYPEDEFS(StateBlock);
WOLF_PTR_TYPEDEFS(StatePoint2d);
WOLF_PTR_TYPEDEFS(StatePoint3d);
WOLF_PTR_TYPEDEFS(StateAngle);
WOLF_PTR_TYPEDEFS(StateQuaternion);
......@@ -310,12 +317,16 @@ WOLF_PTR_TYPEDEFS(LocalParametrizationBase);
inline bool file_exists(const std::string& _name)
{
struct stat buffer;
return (stat (_name.c_str(), &buffer) == 0);
return (stat(_name.c_str(), &buffer) == 0);
}
inline const Eigen::Vector3d gravity(void) {
return Eigen::Vector3d(0,0,-9.806);
inline const Eigen::Vector3d gravity(void)
{
return Eigen::Vector3d(0, 0, -9.806);
}
} // namespace wolf
#endif /* WOLF_H_ */
// ===================================================
// Schema folder registry
WOLF_REGISTER_FOLDER("core", _WOLF_SCHEMA_DIR);
} // namespace wolf
// 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/>.
#pragma once
#include "core/common/wolf.h"
#include <string>
#include <map>
#include <algorithm>
#include "yaml-cpp/yaml.h"
namespace wolf
{
using std::map;
typedef std::string StateKeys;
static std::string ALL_KEYS = " ";
template <typename T>
class Composite : public map<char, T>
{
public:
using CompositeType = T;
using map<char, T>::map;
Composite(const YAML::Node& _n,
const std::string& _field,
const bool _required,
const std::string& _keys = ALL_KEYS);
virtual ~Composite() = default;
bool has(char _key) const;
bool has(const StateKeys& _structure) const;
StateKeys getKeys() const;
friend std::ostream& operator<<(std::ostream& _os, const Composite<T>& _x)
{
for (auto&& pair : _x)
{
_os << pair.first << ": " << pair.second << std::endl;
}
return _os;
}
Composite operator()(const std::string& _keys) const;
YAML::Node toYaml() const;
};
typedef Composite<bool> BoolComposite;
template <typename T>
inline Composite<T>::Composite(const YAML::Node& _n,
const std::string& _field,
const bool _required,
const std::string& _keys)
{
if (_n.IsDefined())
{
if (not _n.IsMap())
{
throw std::runtime_error("Composite: constructor with a non-map yaml node");
}
// check existence of _keys in node
if (_keys != ALL_KEYS)
for (auto key : _keys)
if (not _n[key] and _required)
{
throw std::runtime_error(std::string("Composite ") + typeid(T).name() +
" constructor: Missing node for key '" + key + "'.");
}
// iterate node map pairs
for (auto node_pair : _n)
{
// check that keys are char type
char key;
try
{
key = node_pair.first.as<char>();
}
catch (const std::exception& e)
{
throw std::runtime_error(std::string("In Composite ") + typeid(T).name() +
" constructor: There is a key of type different than char: " +
node_pair.first.as<std::string>() + ". Error: " + e.what());
}
// discard if not in _keys (if _keys is not ALL_KEYS)
if (_keys != ALL_KEYS and //
_keys.find(key) == std::string::npos) //
continue;
// check field
if (not _field.empty())
{
if (node_pair.second[_field])
{
try
{
this->emplace(key, node_pair.second[_field].as<T>());
}
catch (const std::exception& e)
{
YAML::Emitter em;
em << node_pair.second[_field];
throw std::runtime_error("In Composite constructor: Failed to emplace the entry " + _field +
" for key '" + key + "' with error: " + e.what() + "\nNode:\n" +
em.c_str());
}
}
else if (_required)
{
YAML::Emitter em;
em << node_pair.second;
throw std::runtime_error("In Composite constructor: Missing mandatory field '" + _field +
"' for key '" + key + "'\nNode:\n" + em.c_str());
}
}
else
{
try
{
this->emplace(key, node_pair.second.as<T>());
}
catch (const std::exception& e)
{
YAML::Emitter em;
em << node_pair.second;
throw std::runtime_error("In Composite constructor: Failed to emplace the entry for key '" +
std::string(1, key) + "' with error: " + e.what() + "\nNode:\n" +
em.c_str());
}
}
}
}
else
{
WOLF_WARN("The YAML node is not defined. Constructing an empty composite");
}
}
template <typename T>
inline bool Composite<T>::has(char _key) const
{
return this->count(_key) != 0;
}
template <typename T>
inline bool Composite<T>::has(const StateKeys& _query_keys) const
{
for (auto&& key : _query_keys)
{
if (not has(key))
{
return false;
}
}
return true;
}
template <typename T>
inline StateKeys Composite<T>::getKeys() const
{
StateKeys keys;
for (auto&& pair : *this) keys.push_back(pair.first);
return keys;
}
template <typename T>
inline Composite<T> Composite<T>::operator()(const std::string& _keys) const
{
if (not this->has(_keys))
{
throw std::runtime_error("Composite<T>::operator() required keys " + _keys + " are not available, only have " +
getKeys());
}
Composite<T> output;
for (auto key : _keys)
{
output.emplace(key, this->at(key));
}
return output;
}
template <typename T>
inline YAML::Node Composite<T>::toYaml() const
{
return YAML::Node(*this);
}
} // namespace wolf
// CONVERSION TO YAML
namespace YAML
{
template <typename T>
struct convert<wolf::Composite<T>>
{
static Node encode(const wolf::Composite<T>& composite)
{
Node n;
for (auto&& pair : composite)
{
n[pair.first] = pair.second;
}
return n;
}
static bool decode(const Node& node, wolf::Composite<T>& composite)
{
composite = wolf::Composite<T>(node);
return true;
}
};
} // namespace YAML
// 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/>.
#pragma once
#include "core/composite/composite.h"
#include <string>
#include <eigen3/Eigen/Dense>
#include "yaml-cpp/yaml.h"
namespace wolf
{
class Prior
{
protected:
std::string prior_mode_; // Prior mode can be 'initial_guess', 'fix' or 'factor'
Eigen::VectorXd factor_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix
// (ONLY IF prior_mode_ == 'factor')
public:
Prior() = default;
Prior(const std::string& _prior_mode, const Eigen::VectorXd& _factor_std = Eigen::VectorXd(0));
Prior(const YAML::Node& _n);
virtual ~Prior() = default;
const std::string& getPriorMode() const;
const Eigen::VectorXd& getFactorStd() const;
bool isInitialGuess() const;
bool isFixed() const;
bool isFactor() const;
virtual void check() const;
friend std::ostream& operator<<(std::ostream& _os, const wolf::Prior& _x);
YAML::Node toYaml() const;
};
typedef Composite<Prior> PriorComposite;
inline const std::string& Prior::getPriorMode() const
{
return prior_mode_;
}
inline const Eigen::VectorXd& Prior::getFactorStd() const
{
return factor_std_;
}
inline bool Prior::isInitialGuess() const
{
return prior_mode_ == "initial_guess";
}
inline bool Prior::isFixed() const
{
return prior_mode_ == "fix";
}
inline bool Prior::isFactor() const
{
return prior_mode_ == "factor";
}
} // namespace wolf
// CONVERSION TO YAML
namespace YAML
{
template <>
struct convert<wolf::Prior>
{
static Node encode(const wolf::Prior& spec)
{
return spec.toYaml();
}
static bool decode(const Node& node, wolf::Prior& spec)
{
spec = wolf::Prior(node);
return true;
}
};
} // namespace YAML
// 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/>.
#pragma once
#include "core/common/wolf.h"
#include "core/composite/composite.h"
namespace wolf
{
typedef Composite<std::string> TypeComposite;
void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim = 0);
bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim = 0);
inline bool isTypeCompositeValid(const TypeComposite& _types, SizeEigen _dim)
{
try
{
checkTypeComposite(_types, _dim);
}
catch (const std::exception& e)
{
return false;
}
return true;
}
inline void checkTypeComposite(const TypeComposite& _types, SizeEigen _dim)
{
if (_dim == 2)
{
if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'P' in 2D should be 'StatePoint2d'");
}
if (_types.count('O') != 0 and _types.at('O') != "StateAngle")
{
throw std::runtime_error("Bad TypeComposite: Type for 'O' in 2D should be 'StateAngle'");
}
if (_types.count('V') != 0 and _types.at('V') != "StateVector2d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'V' in 2D should be 'StateVector2d'");
}
}
else if (_dim == 3)
{
if (_types.count('P') != 0 and _types.at('P') != "StatePoint3d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'P' in 3D should be 'StatePoint3d'");
}
if (_types.count('O') != 0 and _types.at('O') != "StateQuaternion")
{
throw std::runtime_error("Bad TypeComposite: Type for 'O' in 3D should be 'StateQuaternion'");
}
if (_types.count('V') != 0 and _types.at('V') != "StateVector3d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'V' in 3D should be 'StateVector3d'");
}
}
else if (_dim == 0)
{
if (_types.count('P') != 0 and _types.at('P') != "StatePoint2d" and _types.at('P') != "StatePoint3d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'P' should be 'StatePoint2d' or 'StatePoint3d'");
}
if (_types.count('O') != 0 and _types.at('O') != "StateAngle" and _types.at('O') != "StateQuaternion")
{
throw std::runtime_error("Bad TypeComposite: Type for 'O' should be 'StateAngle' or 'StateQuaternion'");
}
if (_types.count('V') != 0 and _types.at('V') != "StateVector2d" and _types.at('V') != "StateVector3d")
{
throw std::runtime_error("Bad TypeComposite: Type for 'V' should be 'StateVector2d' or 'StateVector3d'");
}
}
else
{
throw std::runtime_error(
"checkTypeComposite(_types, _dim) bad value for '_dim', should be 2 for 2D, 3 for 3D, or 0 for not "
"defined");
}
}
} // namespace wolf
// 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/>.
#pragma once
#include "core/common/wolf.h"
#include "core/composite/composite.h"
namespace wolf
{
class VectorComposite : public Composite<Eigen::VectorXd>
{
public:
using Composite<Eigen::VectorXd>::Composite;
VectorComposite(const Composite<Eigen::VectorXd>& _composite);
Eigen::VectorXd vector(const StateKeys& _keys) const;
friend std::ostream& operator<<(std::ostream& _os, const wolf::VectorComposite& _x);
};
} // namespace wolf
//--------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/>.
#pragma once
#ifndef FACTOR_ANALYTIC_H_
#define FACTOR_ANALYTIC_H_
//Wolf includes
// Wolf includes
#include "core/factor/factor_base.h"
#include <Eigen/StdVector>
namespace wolf {
WOLF_PTR_TYPEDEFS(FactorAnalytic);
class FactorAnalytic: public FactorBase
namespace wolf
{
protected:
std::vector<StateBlockPtr> state_ptr_vector_;
std::vector<StateBlockConstPtr> state_ptr_const_vector_;
std::vector<unsigned int> state_block_sizes_vector_;
public:
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 = nullptr,
StateBlockPtr _state2Ptr = nullptr,
StateBlockPtr _state3Ptr = nullptr,
StateBlockPtr _state4Ptr = nullptr,
StateBlockPtr _state5Ptr = nullptr,
StateBlockPtr _state6Ptr = nullptr,
StateBlockPtr _state7Ptr = nullptr,
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
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 = nullptr,
StateBlockPtr _state2Ptr = nullptr,
StateBlockPtr _state3Ptr = nullptr,
StateBlockPtr _state4Ptr = nullptr,
StateBlockPtr _state5Ptr = nullptr,
StateBlockPtr _state6Ptr = nullptr,
StateBlockPtr _state7Ptr = nullptr,
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
~FactorAnalytic() override = default;
/** \brief Returns a vector of pointers to the states
*
* Returns a vector of pointers to the state in which this factor depends
*
**/
std::vector<StateBlockConstPtr> getStateBlockPtrVector() const override;
std::vector<StateBlockPtr> getStateBlockPtrVector() override;
/** \brief Returns a vector of sizes of the state blocks
*
* Returns a vector of sizes of the state blocks
*
**/
std::vector<unsigned int> getStateSizes() const override;
/** \brief Returns the factor residual size
*
* Returns the factor residual size
*
**/
// virtual unsigned int getSize() const = 0;
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
state_blocks_map_.emplace_back((double*)parameters[i], state_block_sizes_vector_[i]);
// residuals
Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
residuals_map = evaluateResiduals(state_blocks_map_);
// also compute jacobians
if (jacobians != nullptr)
{
std::vector<Eigen::Map<Eigen::MatrixRowXd>> jacobians_map_;
std::vector<bool> compute_jacobians_(state_block_sizes_vector_.size());
for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
{
compute_jacobians_[i] = (jacobians[i] != nullptr);
if (jacobians[i] != nullptr)
jacobians_map_.emplace_back((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]);
else
jacobians_map_.emplace_back(nullptr, 0, 0); //TODO: check if it can be done
}
// evaluate jacobians
evaluateJacobians(state_blocks_map_, jacobians_map_, compute_jacobians_);
}
return true;
};
/** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
assert(_states_ptr.size() == state_block_sizes_vector_.size());
// load parameters evaluation value
std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
state_blocks_map_.emplace_back(_states_ptr[i], state_block_sizes_vector_[i]);
// residuals
residual_ = evaluateResiduals(state_blocks_map_);
// compute jacobians
jacobians_.resize(state_block_sizes_vector_.size());
evaluateJacobians(state_blocks_map_, jacobians_, std::vector<bool>(state_block_sizes_vector_.size(),true));
};
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const = 0;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const = 0;
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const = 0;
/** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
* Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const = 0;
/** \brief Returns the jacobians computation method
*
* Returns the jacobians computation method
*
**/
JacobianMethod getJacobianMethod() const override;
WOLF_PTR_TYPEDEFS(FactorAnalytic);
private:
void resizeVectors();
class FactorAnalytic : public FactorBase
{
public:
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);
~FactorAnalytic() override = default;
/** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override;
/** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the
*point provided in _states_ptr
**/
bool evaluate(const std::vector<const double*>& _states_ptr,
Eigen::VectorXd& _residual,
std::vector<Eigen::MatrixXd>& _jacobians) const override;
/** \brief Returns the residual evaluated in the states provided
*
* Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
virtual Eigen::VectorXd evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const = 0;
/** \brief Returns the normalized jacobians evaluated in the states
*
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in
*std::vector of mapped Eigen::VectorXd. IMPORTANT: only fill the jacobians of the state blocks specified in
*_compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixRowXd>>& jacobians,
const std::vector<bool>& _compute_jacobian) const = 0;
virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector,
std::vector<Eigen::MatrixXd>& jacobians,
const std::vector<bool>& _compute_jacobian) const = 0;
/** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks
*values
*
* Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state
*block
*
**/
virtual void evaluateUnweightedJacobians(std::vector<Eigen::MatrixXd>& jacobians) const = 0;
};
} // namespace wolf
#endif
} // namespace wolf