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 815 additions and 3546 deletions
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
#ifndef SENSOR_BASE_H_ // along with this program. If not, see <http://www.gnu.org/licenses/>.
#define SENSOR_BASE_H_ #pragma once
// Fwd refs // Fwd refs
namespace wolf{ namespace wolf
{
class HardwareBase; class HardwareBase;
class ProcessorBase; class ProcessorBase;
class StateBlock; class StateBlock;
} } // namespace wolf
//Wolf includes // Wolf includes
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include "core/common/node_base.h" #include "core/sensor/factory_sensor.h"
#include "core/common/node_state_blocks.h"
#include "core/common/time_stamp.h" #include "core/common/time_stamp.h"
#include "core/common/params_base.h" #include "yaml-cpp/yaml.h"
#include "core/state_block/has_state_blocks.h" #include "yaml-schema-cpp/yaml_schema.hpp"
//std includes // std includes
namespace wolf {
namespace wolf
{
/* /*
* Macro for defining Autoconf sensor creator. * Macro for defining Autoconf sensor creator.
* *
* Place a call to this macro inside your class declaration (in the sensor_class.h file), * Place a call to this macro inside your class declaration (in the sensor_class.h file),
* preferably just after the constructors. * preferably just after the constructors. Inside 'public' keyword.
* *
* In order to use this macro, the derived sensor class, SensorClass, * In order to use this macro, the derived sensor class, SensorClass,
* must have a constructor available with the API: * must have two constructors available with the API:
* *
* SensorClass(const VectorXd & _extrinsics, const ParamsSensorClassPtr _intrinsics); * SensorClass(const YAML::Node& _params);
* *
* We recommend writing one of such constructors in your derived sensors. * Also, there should be the schema file 'SensorClass.schema' containing the specifications
* of the user input yaml file.
*/ */
#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \ // We use `new` and not `make_shared` since the constructor may (should) be private/protected
static \ #define WOLF_SENSOR_CREATE(SensorClass) \
SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \ static SensorBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
{ \ { \
Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \ if (not _folders_schema.empty()) \
assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ { \
auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
auto sensor = std::make_shared<SensorClass>(extrinsics, params); \ server.setYaml(_input_node); \
sensor ->setName(_unique_name); \ if (not server.applySchema(#SensorClass)) \
return sensor; \ { \
} \ WOLF_ERROR(server.getLog()); \
\ return nullptr; \
static \ } \
SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics)\ } \
{ \ std::shared_ptr<SensorClass> sen(new SensorClass(_input_node)); \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ sen->emplacePriors(); \
auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \ return sen->shared_from_this_sensor(); \
auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \ } \
sensor ->setName(_unique_name); \ static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
return sensor; \ { \
} \ auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#SensorClass)) \
{ \
/** \brief base struct for intrinsic sensor parameters WOLF_ERROR(server.getLog()); \
* return nullptr; \
* Derive from this struct to create structs of sensor intrinsic parameters. } \
*/ return create(server.getNode(), {}); \
struct ParamsSensorBase: public ParamsBase }
{
std::string prefix = "sensor/"; #define WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorClass, Dim) \
~ParamsSensorBase() override = default; static SensorBasePtr create(const YAML::Node& _input_node, const std::vector<std::string>& _folders_schema = {}) \
using ParamsBase::ParamsBase; { \
std::string print() const override if (not _folders_schema.empty()) \
{ { \
return ""; auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
if (not server.applySchema(#SensorClass + std::to_string(Dim) + "d")) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
std::shared_ptr<SensorClass> sen(new SensorClass<Dim>(_input_node)); \
sen->emplacePriors(); \
return sen->shared_from_this_sensor(); \
} \
static SensorBasePtr create(const std::string& _yaml_filepath, const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#SensorClass + std::to_string(Dim) + "d")) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(server.getNode(), {}); \
} }
};
class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_shared_from_this<SensorBase> WOLF_PTR_TYPEDEFS(SensorBase);
class SensorBase : public NodeStateBlocks
{ {
friend Problem; friend Problem;
friend ProcessorBase; friend ProcessorBase;
private: private:
HardwareBaseWPtr hardware_ptr_; bool enabled_; ///< Sensor enabled
ProcessorBasePtrList processor_list_; HardwareBaseWPtr hardware_ptr_;
ProcessorBasePtrList processor_list_;
static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory) static unsigned int sensor_id_count_; ///< Object counter (acts as simple ID factory)
unsigned int sensor_id_; ///< sensor ID
unsigned int sensor_id_; // sensor ID std::map<char, bool> state_block_dynamic_; ///< mark dynamic state blocks
CaptureBasePtr last_capture_; ///< last capture of the sensor (in the WOLF tree)
std::map<char, bool> state_block_dynamic_; // mark dynamic state blocks
protected:
std::map<char, FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by key in state_block_map_) std::map<char, Eigen::VectorXd>
drifts_std_rate_; ///< std rate of the drift of dynamic state blocks [unit/sqrt(s)]
CaptureBasePtr last_capture_; // last capture of the sensor (in the WOLF tree) YAML::Node params_; ///< Params yaml node
protected: void setProblem(ProblemPtr _problem) override final;
Eigen::VectorXd noise_std_; // std of sensor noise
Eigen::MatrixXd noise_cov_; // cov matrix of noise /** \brief Constructor
*
void setProblem(ProblemPtr _problem) override final; * \param _type TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: derived class name
* \param _state_types TO BE HARDCODED IN THE DERIVED CLASS CONSTRUCTOR: Composite of keys and types of the states
public: * of the sensor.
/** \brief Constructor with noise size * \param _params params yaml node
* *
* Constructor with parameter vector **/
* \param _tp Type of the sensor (types defined at wolf.h) SensorBase(const std::string& _type, const TypeComposite& _state_types, const YAML::Node& _params);
* \param _p_ptr StateBlock pointer to the sensor position
* \param _o_ptr StateBlock pointer to the sensor orientation public:
* \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed). ~SensorBase() override;
* \param _noise_size dimension of the noise term
* \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving) unsigned int id() const override;
* \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving)
* \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving) HardwareBaseConstPtr getHardware() const;
* HardwareBasePtr getHardware();
**/
SensorBase(const std::string& _type, YAML::Node getParams() const;
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr, private:
StateBlockPtr _intr_ptr, void setHardware(const HardwareBasePtr _hw_ptr);
const unsigned int _noise_size, ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
const bool _p_dyn = false, void removeProcessor(ProcessorBasePtr _proc_ptr);
const bool _o_dyn = false,
const bool _intr_dyn = false); public:
ProcessorBaseConstPtrList getProcessorList() const;
/** \brief Constructor with noise std vector ProcessorBasePtrList getProcessorList();
*
* Constructor with parameter vector CaptureBaseConstPtr getLastCapture() const;
* \param _tp Type of the sensor (types defined at wolf.h) CaptureBasePtr getLastCapture();
* \param _p_ptr StateBlock pointer to the sensor position void setLastCapture(CaptureBasePtr);
* \param _o_ptr StateBlock pointer to the sensor orientation void updateLastCapture();
* \param _intr_ptr StateBlock pointer to the sensor intrinsic params that might be estimated (if unfixed). CaptureBaseConstPtr findLastCaptureBefore(const TimeStamp& _ts) const;
* \param _noise_std standard deviations of the noise term CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts);
* \param _p_dyn Flag indicating if position is dynamic (moving) or static (not moving)
* \param _o_dyn Flag indicating if orientation is dynamic (moving) or static (not moving) bool process(const CaptureBasePtr capture_ptr);
* \param _intr_dyn Flag indicating if intrinsics is dynamic (moving) or static (not moving)
* // State blocks
**/ StateBlockConstPtr getStateBlock(const char& _key) const override;
SensorBase(const std::string& _type, StateBlockPtr getStateBlock(const char& _key) override;
StateBlockPtr _p_ptr, StateBlockConstPtr getStateBlock(const char& _key, const TimeStamp& _ts) const;
StateBlockPtr _o_ptr, StateBlockPtr getStateBlock(const char& _key, const TimeStamp& _ts);
StateBlockPtr _intr_ptr,
const Eigen::VectorXd & _noise_std, // get capture where the state block is
const bool _p_dyn = false, CaptureBaseConstPtr getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts) const;
const bool _o_dyn = false, CaptureBasePtr getCaptureContainingStateBlock(const char& _key, const TimeStamp& _ts);
const bool _intr_dyn = false); CaptureBaseConstPtr getCaptureContainingStateBlock(const char& _key) const;
CaptureBasePtr getCaptureContainingStateBlock(const char& _key);
~SensorBase() override;
StateBlockConstPtr getP(const TimeStamp _ts) const;
unsigned int id() const; StateBlockPtr getP(const TimeStamp _ts);
StateBlockConstPtr getO(const TimeStamp _ts) const;
HardwareBaseConstPtr getHardware() const; StateBlockPtr getO(const TimeStamp _ts);
HardwareBasePtr getHardware(); StateBlockConstPtr getIntrinsic(const TimeStamp _ts) const;
StateBlockPtr getIntrinsic(const TimeStamp _ts);
private: StateBlockConstPtr getP() const;
void setHardware(const HardwareBasePtr _hw_ptr); StateBlockPtr getP();
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr); StateBlockConstPtr getO() const;
void removeProcessor(ProcessorBasePtr _proc_ptr); StateBlockPtr getO();
StateBlockConstPtr getIntrinsic() const;
public: StateBlockPtr getIntrinsic();
ProcessorBaseConstPtrList getProcessorList() const;
ProcessorBasePtrList getProcessorList(); // Declare/get dynamic
void setStateBlockDynamic(const char& _key, bool _dynamic = true);
CaptureBaseConstPtr getLastCapture() const; bool isStateBlockDynamic(const char& _key) const;
CaptureBasePtr getLastCapture();
void setLastCapture(CaptureBasePtr); // drift rates
void updateLastCapture(); bool hasDrift(char _key) const;
CaptureBaseConstPtr findLastCaptureBefore(const TimeStamp& _ts) const; void setDriftStdRate(char _key, const Eigen::VectorXd& _drift_rate_std);
CaptureBasePtr findLastCaptureBefore(const TimeStamp& _ts); Eigen::VectorXd getDriftStdRate(char) const;
bool process(const CaptureBasePtr capture_ptr); // Set the state block as fixed or unfixed
void fixExtrinsics();
// State blocks void unfixExtrinsics();
using HasStateBlocks::addStateBlock; void fixIntrinsics();
StateBlockPtr addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic = false); void unfixIntrinsics();
StateBlockConstPtr getStateBlockDynamic(const char& _key) const;
StateBlockPtr getStateBlockDynamic(const char& _key); // enable/disable the sensor
StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const; bool isEnabled() const;
StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts); void enable();
void disable();
// Declare a state block as dynamic or static (with _dymanic = false)
void setStateBlockDynamic(const char& _key, bool _dynamic = true); // noise
virtual Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const = 0;
bool isStateBlockDynamic(const char& _key) const;
bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBaseConstPtr& _cap) const; virtual void printHeader(int depth, //
bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts, CaptureBasePtr& _cap); bool factored_by, //
bool isStateBlockInCapture(const char& _key, CaptureBaseConstPtr& _cap) const; bool metric, //
bool isStateBlockInCapture(const char& _key, CaptureBasePtr& _cap); bool state_blocks,
bool isStateBlockInCapture(const char& _key, const TimeStamp& _ts) const; std::ostream& stream,
bool isStateBlockInCapture(const char& _key) const; std::string _tabs = "") const;
void printState(bool _factored_by,
StateBlockConstPtr getP(const TimeStamp _ts) const; bool _metric,
StateBlockPtr getP(const TimeStamp _ts); bool _state_blocks,
StateBlockConstPtr getO(const TimeStamp _ts) const; std::ostream& _stream,
StateBlockPtr getO(const TimeStamp _ts); std::string _tabs) const;
StateBlockConstPtr getIntrinsic(const TimeStamp _ts) const;
StateBlockPtr getIntrinsic(const TimeStamp _ts); void print(int depth, //
StateBlockConstPtr getP() const; bool factored_by, //
StateBlockPtr getP(); bool metric, //
StateBlockConstPtr getO() const; bool state_blocks,
StateBlockPtr getO(); std::ostream& stream = std::cout,
StateBlockConstPtr getIntrinsic() const; std::string _tabs = "") const;
StateBlockPtr getIntrinsic();
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
protected: bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
virtual void registerNewStateBlocks(ProblemPtr _problem) override;
void link(HardwareBasePtr);
public: template <typename classType>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr,
void fixExtrinsics(); const YAML::Node& _params,
void unfixExtrinsics(); const std::vector<std::string>& _folders_schema = {});
void fixIntrinsics();
void unfixIntrinsics(); SensorBasePtr shared_from_this_sensor()
{
/** \brief Add an absolute factor to a parameter return std::static_pointer_cast<SensorBase>(shared_from_this());
* };
* Add an absolute factor to a parameter SensorBaseConstPtr shared_from_this_sensor() const
* \param _i state block index (in state_block_vec_) of the parameter to be constrained {
* \param _x prior value return std::static_pointer_cast<const SensorBase>(shared_from_this());
* \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 addPriorParameter(const char& _key,
const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addPriorP(const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void addPriorO(const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov);
void addPriorIntrinsics(const Eigen::VectorXd& _x,
const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
void setNoiseStd(const Eigen::VectorXd & _noise_std);
void setNoiseCov(const Eigen::MatrixXd & _noise_std);
Eigen::VectorXd getNoiseStd() const;
Eigen::MatrixXd getNoiseCov() const;
virtual void printHeader(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
void printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
void print(int depth, //
bool constr_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, SensorBaseConstPtr _sen_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
void link(HardwareBasePtr);
template<typename classType, typename... T>
static std::shared_ptr<classType> emplace(HardwareBasePtr _hwd_ptr, T&&... all);
}; };
} } // namespace wolf
#include "core/problem/problem.h"
#include "core/hardware/hardware_base.h" #include "core/hardware/hardware_base.h"
#include "core/capture/capture_base.h" #include "core/capture/capture_base.h"
#include "core/processor/processor_base.h" #include "core/processor/processor_base.h"
namespace wolf{ namespace wolf
template<typename classType, typename... T>
std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr, T&&... all)
{ {
std::shared_ptr<classType> sen = std::make_shared<classType>(std::forward<T>(all)...); template <typename classType>
std::shared_ptr<classType> SensorBase::emplace(HardwareBasePtr _hwd_ptr,
const YAML::Node& _params,
const std::vector<std::string>& _folders_schema)
{
std::shared_ptr<classType> sen = std::static_pointer_cast<classType>(classType::create(_params, _folders_schema));
sen->emplacePriors();
sen->link(_hwd_ptr); sen->link(_hwd_ptr);
return sen; return sen;
} }
...@@ -307,8 +282,7 @@ inline unsigned int SensorBase::id() const ...@@ -307,8 +282,7 @@ inline unsigned int SensorBase::id() const
inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const inline ProcessorBaseConstPtrList SensorBase::getProcessorList() const
{ {
ProcessorBaseConstPtrList list_const; ProcessorBaseConstPtrList list_const;
for (auto&& obj_ptr : processor_list_) for (auto&& obj_ptr : processor_list_) list_const.push_back(obj_ptr);
list_const.push_back(obj_ptr);
return list_const; return list_const;
} }
...@@ -317,44 +291,49 @@ inline ProcessorBasePtrList SensorBase::getProcessorList() ...@@ -317,44 +291,49 @@ inline ProcessorBasePtrList SensorBase::getProcessorList()
return processor_list_; return processor_list_;
} }
inline CaptureBaseConstPtr SensorBase::getLastCapture() const inline YAML::Node SensorBase::getParams() const
{ {
return last_capture_; return Clone(params_);
} }
inline CaptureBasePtr SensorBase::getLastCapture() inline CaptureBaseConstPtr SensorBase::getLastCapture() const
{ {
return last_capture_; return last_capture_;
} }
inline StateBlockPtr SensorBase::addStateBlock(const char& _key, const StateBlockPtr& _sb_ptr, const bool _dynamic) inline CaptureBasePtr SensorBase::getLastCapture()
{ {
assert((params_prior_map_.find(_key) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor"); return last_capture_;
HasStateBlocks::addStateBlock(_key, _sb_ptr, getProblem());
setStateBlockDynamic(_key, _dynamic);
return _sb_ptr;
} }
inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic) inline void SensorBase::setStateBlockDynamic(const char& _key, bool _dynamic)
{ {
assert(hasStateBlock(_key) and "setting dynamic an state block of a key that doesn't exist in the sensor. It is required anyway."); if (not has(_key))
throw std::runtime_error(std::string("SensorBase::setStateBlockDynamic: key ") + _key +
" doesn't exist in the sensor.");
state_block_dynamic_[_key] = _dynamic; state_block_dynamic_[_key] = _dynamic;
} }
inline bool SensorBase::isStateBlockDynamic(const char& _key) const inline bool SensorBase::isStateBlockDynamic(const char& _key) const
{ {
assert(state_block_dynamic_.count(_key) != 0); if (state_block_dynamic_.count(_key) == 0)
throw std::runtime_error(std::string("SensorBase::isStateBlockDynamic: key ") + _key +
" not found in 'state_block_dynamic_'.");
return state_block_dynamic_.at(_key); return state_block_dynamic_.at(_key);
} }
inline Eigen::VectorXd SensorBase::getNoiseStd() const inline Eigen::VectorXd SensorBase::getDriftStdRate(char _key) const
{ {
return noise_std_; if (drifts_std_rate_.count(_key) == 0)
return Eigen::VectorXd(0);
else
return drifts_std_rate_.at(_key);
} }
inline Eigen::MatrixXd SensorBase::getNoiseCov() const inline bool SensorBase::hasDrift(char _key) const
{ {
return noise_cov_; return drifts_std_rate_.count(_key);
} }
inline HardwareBaseConstPtr SensorBase::getHardware() const inline HardwareBaseConstPtr SensorBase::getHardware() const
...@@ -372,21 +351,19 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr) ...@@ -372,21 +351,19 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr; hardware_ptr_ = _hw_ptr;
} }
inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) inline bool SensorBase::isEnabled() const
{ {
addPriorParameter('P', _x, _cov, _start_idx, _size); return enabled_;
} }
inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov) inline void SensorBase::enable()
{ {
addPriorParameter('O', _x, _cov); enabled_ = true;
} }
inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size) inline void SensorBase::disable()
{ {
addPriorParameter('I', _x, _cov); enabled_ = false;
} }
} // namespace wolf } // namespace wolf
#endif
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/** // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file sensor_diff_drive.h #pragma once
*
* Created on: Jul 22, 2019
* \author: jsola
*/
#ifndef SENSOR_SENSOR_DIFF_DRIVE_H_
#define SENSOR_SENSOR_DIFF_DRIVE_H_
#include "core/sensor/sensor_base.h" #include "core/sensor/sensor_base.h"
namespace wolf namespace wolf
{ {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive);
struct ParamsSensorDiffDrive : public ParamsSensorBase
{
double radius_left;
double radius_right;
double wheel_separation;
double ticks_per_wheel_revolution;
bool set_intrinsics_prior;
Eigen::Vector3d prior_cov_diag;
double ticks_cov_factor;
ParamsSensorDiffDrive() = default;
ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server)
: ParamsSensorBase(_unique_name, _server)
{
radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left");
radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right");
wheel_separation = _server.getParam<double>(prefix + _unique_name + "/wheel_separation");
ticks_per_wheel_revolution = _server.getParam<double>(prefix + _unique_name + "/ticks_per_wheel_revolution");
set_intrinsics_prior = _server.getParam<bool>(prefix + _unique_name + "/set_intrinsics_prior");
prior_cov_diag = _server.getParam<Eigen::Vector3d>(prefix + _unique_name + "/prior_cov_diag");
ticks_cov_factor = _server.getParam<double>(prefix + _unique_name + "/ticks_cov_factor");
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
+ "radius_left: " + std::to_string(radius_left) + "\n"
+ "radius_right: " + std::to_string(radius_right) + "\n"
+ "wheel_separation: " + std::to_string(wheel_separation) + "\n"
+ "ticks_per_wheel_revolution: " + std::to_string(ticks_per_wheel_revolution)+ "\n"
+ "set_intrinsics_prior: " + std::to_string(set_intrinsics_prior) + "\n"
+ "prior_cov_diag: to_string not implemented yet! \n"
+ "ticks_cov_factor: " + std::to_string(ticks_cov_factor) + "\n";
}
};
WOLF_PTR_TYPEDEFS(SensorDiffDrive); WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase class SensorDiffDrive : public SensorBase
{ {
public: SensorDiffDrive(const YAML::Node& _params);
SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
ParamsSensorDiffDrivePtr _intrinsics);
WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive, 3);
~SensorDiffDrive() override;
ParamsSensorDiffDriveConstPtr getParams() const;
double getRadiansPerTick() const public:
{ WOLF_SENSOR_CREATE(SensorDiffDrive);
return radians_per_tick;
}
protected: ~SensorDiffDrive() override = default;
ParamsSensorDiffDrivePtr params_diff_drive_;
double radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM.
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
double getTicksPerWheelRevolution() const;
double getTicksStdFactor() const;
double getRadiansPerTick() const;
protected:
// params
double ticks_per_wheel_revolution_;
double ticks_std_factor_;
}; };
inline wolf::ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const inline double SensorDiffDrive::getTicksPerWheelRevolution() const
{ {
return params_diff_drive_; return ticks_per_wheel_revolution_;
} }
} /* namespace wolf */ inline double SensorDiffDrive::getTicksStdFactor() const
{
return ticks_std_factor_;
}
#endif /* SENSOR_SENSOR_DIFF_DRIVE_H_ */ inline double SensorDiffDrive::getRadiansPerTick() const
{
return 2.0 * M_PI / ticks_per_wheel_revolution_;
}
} /* namespace wolf */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
#ifndef SRC_SENSOR_MOTION_MODEL_H_ // along with this program. If not, see <http://www.gnu.org/licenses/>.
#define SRC_SENSOR_MOTION_MODEL_H_ #pragma once
//wolf includes // wolf includes
#include "core/sensor/sensor_base.h" #include "core/sensor/sensor_base.h"
namespace wolf { namespace wolf
{
WOLF_PTR_TYPEDEFS(SensorMotionModel); WOLF_PTR_TYPEDEFS(SensorMotionModel);
class SensorMotionModel : public SensorBase class SensorMotionModel : public SensorBase
{ {
public: SensorMotionModel(const YAML::Node& _params);
SensorMotionModel();
~SensorMotionModel() override;
static SensorBasePtr create(const std::string& _unique_name, public:
const ParamsServer& _server) WOLF_SENSOR_CREATE(SensorMotionModel);
{
auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
static SensorBasePtr create(const std::string& _unique_name, ~SensorMotionModel() override;
const Eigen::VectorXd& _extrinsics,
const ParamsSensorBasePtr _intrinsics)
{
auto sensor = std::make_shared<SensorMotionModel>();
sensor ->setName(_unique_name);
return sensor;
}
};
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
} /* namespace wolf */ } /* namespace wolf */
#endif /* SRC_SENSOR_MOTION_MODEL_H_ */
// 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
// wolf includes
#include "core/sensor/sensor_base.h"
#include "yaml-cpp/yaml.h"
namespace wolf
{
template <unsigned int DIM>
class SensorOdom : public SensorBase
{
protected:
SensorOdom(const YAML::Node& _params)
: SensorBase(
"SensorOdom" + std::to_string(DIM) + "d",
{{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
_params)
{
static_assert(DIM == 2 or DIM == 3);
}
public:
WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorOdom, DIM);
~SensorOdom() override = default;
double getDispVarToDispNoiseFactor() const;
double getDispVarToRotNoiseFactor() const;
double getRotVarToRotNoiseFactor() const;
double getMinDispVar() const;
double getMinRotVar() const;
Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
};
template <unsigned int DIM>
inline double SensorOdom<DIM>::getDispVarToDispNoiseFactor() const
{
return params_["k_disp_to_disp"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getDispVarToRotNoiseFactor() const
{
return params_["k_disp_to_rot"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getRotVarToRotNoiseFactor() const
{
return params_["k_rot_to_rot"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getMinDispVar() const
{
return params_["min_disp_var"].as<double>();
}
template <unsigned int DIM>
inline double SensorOdom<DIM>::getMinRotVar() const
{
return params_["min_rot_var"].as<double>();
}
template <unsigned int DIM>
inline Eigen::MatrixXd SensorOdom<DIM>::computeNoiseCov(const Eigen::VectorXd& _data) const
{
double d; // displacement
double r; // rotation
// 2D
if (DIM == 2)
{
assert(_data.size() == 2 or _data.size() == 3);
// data = [forward_x, rotation_z] 2D
if (_data.size() == 2)
{
d = fabs(_data(0));
r = fabs(_data(1));
}
else
{
d = _data.head<2>().norm();
r = fabs(_data(2));
}
}
// 3D
else
{
assert(_data.size() == 6 or _data.size() == 7);
d = _data.head<3>().norm();
if (_data.size() == 6)
r = _data.tail<3>().norm();
else
r = 2 * acos(_data(6)); // arc cos of real part of quaternion
}
// variances
double dvar = std::max(this->getMinDispVar(), this->getDispVarToDispNoiseFactor() * d);
double rvar =
std::max(this->getMinRotVar(), this->getDispVarToRotNoiseFactor() * d + this->getRotVarToRotNoiseFactor() * r);
// return
if (DIM == 2)
return (Eigen::Vector2d() << dvar, rvar).finished().asDiagonal();
else
return (Eigen::Vector6d() << dvar, dvar, dvar, rvar, rvar, rvar).finished().asDiagonal();
}
typedef SensorOdom<2> SensorOdom2d;
typedef SensorOdom<3> SensorOdom3d;
WOLF_DECLARED_PTR_TYPEDEFS(SensorOdom2d);
WOLF_DECLARED_PTR_TYPEDEFS(SensorOdom3d);
} /* 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 SENSOR_ODOM_2d_H_
#define SENSOR_ODOM_2d_H_
//wolf includes
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2d);
struct ParamsSensorOdom2d : public ParamsSensorBase
{
~ParamsSensorOdom2d() override = default;
double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
ParamsSensorOdom2d()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsSensorOdom2d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
+ "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n";
}
};
WOLF_PTR_TYPEDEFS(SensorOdom2d);
class SensorOdom2d : public SensorBase
{
protected:
double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
public:
SensorOdom2d(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2d& _intrinsics);
SensorOdom2d(Eigen::VectorXd _extrinsics, ParamsSensorOdom2dPtr _intrinsics);
WOLF_SENSOR_CREATE(SensorOdom2d, ParamsSensorOdom2d, 3);
~SensorOdom2d() override;
/** \brief Returns displacement noise factor
*
* Returns displacement noise factor
*
**/
double getDispVarToDispNoiseFactor() const;
/** \brief Returns rotation noise factor
*
* Returns rotation noise factor
*
**/
double getRotVarToRotNoiseFactor() const;
/**
* Compute covariance of odometry given the motion data.
*
* NOTE: This is a helper function for the user, not called automatically anywhere.
*
* computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
* - a linear dependence with total displacement
* - a linear dependence with total rotation
*
* The formula for the variances is as follows:
*
* - disp_var = k_disp_to_disp * displacement
* - rot_var = k_rot_to_rot * rotation
*
* See implementation for details.
*/
Matrix2d computeCovFromMotion (const VectorXd& _data) const;
};
} // namespace wolf
#endif // SENSOR_ODOM_2d_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)
// 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--------
/**
* \file sensor_odom_3d.h
*
* Created on: Oct 7, 2016
* \author: jsola
*/
#ifndef SRC_SENSOR_ODOM_3d_H_
#define SRC_SENSOR_ODOM_3d_H_
//wolf includes
#include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h"
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3d);
struct ParamsSensorOdom3d : public ParamsSensorBase
{
double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation
double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation
double min_disp_var;
double min_rot_var;
ParamsSensorOdom3d()
{
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
}
ParamsSensorOdom3d(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp");
k_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot");
k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot");
min_disp_var = _server.getParam<double>(prefix + _unique_name + "/min_disp_var");
min_rot_var = _server.getParam<double>(prefix + _unique_name + "/min_rot_var");
}
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
+ "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n"
+ "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n"
+ "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"
+ "min_disp_var: " + std::to_string(min_disp_var) + "\n"
+ "min_rot_var: " + std::to_string(min_rot_var) + "\n";
}
~ParamsSensorOdom3d() override = default;
};
WOLF_PTR_TYPEDEFS(SensorOdom3d);
class SensorOdom3d : public SensorBase
{
protected:
double k_disp_to_disp_; ///< ratio of displacement variance to displacement, for odometry noise calculation
double k_disp_to_rot_; ///< ratio of displacement variance to rotation, for odometry noise calculation
double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
double min_disp_var_;
double min_rot_var_;
public:
SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3d& params);
SensorOdom3d(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3dPtr params);
WOLF_SENSOR_CREATE(SensorOdom3d, ParamsSensorOdom3d, 7);
~SensorOdom3d() override;
double getDispVarToDispNoiseFactor() const;
double getDispVarToRotNoiseFactor() const;
double getRotVarToRotNoiseFactor() const;
double getMinDispVar() const;
double getMinRotVar() const;
/**
* Compute covariance of odometry given the motion data.
*
* NOTE: This is a helper function for the user, not called automatically anywhere.
*
* computeCovFromMotion() produces a diagonal covariance that depends on the amount of motion:
* - a minimal value for displacement
* - a minimal value for rotation
* - a linear dependence with total displacement
* - a linear dependence with total rotation
*
* The formula for the variances is as follows:
*
* - disp_var = disp_var_min + k_disp_to_disp * displacement
* - rot_var = rot_var_min + k_disp_to_rot * displacement + k_rot_to_rot * rotation
*
* See implementation for details.
*/
Matrix6d computeCovFromMotion (const VectorXd& _data) const;
};
inline double SensorOdom3d::getDispVarToDispNoiseFactor() const
{
return k_disp_to_disp_;
}
inline double SensorOdom3d::getDispVarToRotNoiseFactor() const
{
return k_disp_to_rot_;
}
inline double SensorOdom3d::getRotVarToRotNoiseFactor() const
{
return k_rot_to_rot_;
}
inline double SensorOdom3d::getMinDispVar() const
{
return min_disp_var_;
}
inline double SensorOdom3d::getMinRotVar() const
{
return min_rot_var_;
}
} /* namespace wolf */
#endif /* SRC_SENSOR_ODOM_3d_H_ */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/** // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file sensor_pose.h #pragma once
*
* Created on: Feb 18, 2020
* \author: mfourmy
*/
#ifndef SRC_SENSOR_POSE_H_
#define SRC_SENSOR_POSE_H_
//wolf includes // wolf includes
#include "core/sensor/sensor_base.h" #include "core/sensor/sensor_base.h"
#include "core/utils/params_server.h" #include "yaml-cpp/yaml.h"
namespace wolf { namespace wolf
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorPose);
struct ParamsSensorPose : public ParamsSensorBase
{ {
double std_px = 1; // m template <unsigned int DIM>
double std_py = 1; // m class SensorPose : public SensorBase
double std_pz = 1; // m {
double std_ox = 1; // rad protected:
double std_oy = 1; // rad SensorPose(const YAML::Node& _params)
double std_oz = 1; // rad : SensorBase(
ParamsSensorPose() "SensorPose" + std::to_string(DIM) + "d",
{{'P', DIM == 2 ? "StatePoint2d" : "StatePoint3d"}, {'O', DIM == 2 ? "StateAngle" : "StateQuaternion"}},
_params)
{ {
//DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. static_assert(DIM == 2 or DIM == 3);
} }
ParamsSensorPose(std::string _unique_name, const ParamsServer& _server):
ParamsSensorBase(_unique_name, _server)
{
/* Here we allow isotropic specs for position and orientation
* and also particularizations for eaxh axis
* depending on how the info was ptovided in the YAML file.
* Combinations are possible.
*
* E.g. if the yaml file contains
*
* std_p : 1
*
* then the result is std_px = 1, std_py = 1, std_pz = 1
*
* If the yaml file contains (in any order)
*
* std_p : 1
* std_pz : 0.1
*
* then the result is std_px = 1, std_py = 1, std_pz = 0.1
*/
// first the isotropic options public:
if (_server.hasParam(prefix + _unique_name + "/std_p")) WOLF_SENSOR_TEMPLATE_DIM_CREATE(SensorPose, DIM);
std_px = std_py = std_pz = _server.getParam<double>(prefix + _unique_name + "/std_p");
if (_server.hasParam(prefix + _unique_name + "/std_o"))
std_ox = std_oy = std_oz = _server.getParam<double>(prefix + _unique_name + "/std_o");
// then individual axes specifications ~SensorPose() override = default;
if (_server.hasParam(prefix + _unique_name + "/std_px"))
std_px = _server.getParam<double>(prefix + _unique_name + "/std_px");
if (_server.hasParam(prefix + _unique_name + "/std_py"))
std_py = _server.getParam<double>(prefix + _unique_name + "/std_py");
if (_server.hasParam(prefix + _unique_name + "/std_pz"))
std_pz = _server.getParam<double>(prefix + _unique_name + "/std_pz");
if (_server.hasParam(prefix + _unique_name + "/std_ox")) Eigen::VectorXd getStdNoise() const;
std_ox = _server.getParam<double>(prefix + _unique_name + "/std_ox");
if (_server.hasParam(prefix + _unique_name + "/std_oy"))
std_oy = _server.getParam<double>(prefix + _unique_name + "/std_oy");
if (_server.hasParam(prefix + _unique_name + "/std_oz"))
std_oz = _server.getParam<double>(prefix + _unique_name + "/std_oz");
} Eigen::MatrixXd computeNoiseCov(const Eigen::VectorXd& _data) const override;
std::string print() const override
{
return ParamsSensorBase::print() + "\n"
+ "std_px: " + std::to_string(std_px) + "\n"
+ "std_py: " + std::to_string(std_py) + "\n"
+ "std_pz: " + std::to_string(std_pz) + "\n"
+ "std_ox: " + std::to_string(std_ox) + "\n";
+ "std_oy: " + std::to_string(std_oy) + "\n";
+ "std_oz: " + std::to_string(std_oz) + "\n";
}
~ParamsSensorPose() override = default;
}; };
WOLF_PTR_TYPEDEFS(SensorPose); template <unsigned int DIM>
inline Eigen::VectorXd SensorPose<DIM>::getStdNoise() const
class SensorPose : public SensorBase
{ {
protected: return params_["std_noise"].as<Eigen::VectorXd>();
ParamsSensorPosePtr params_pose_; }
public:
SensorPose(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorPose& params);
SensorPose(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorPosePtr params);
WOLF_SENSOR_CREATE(SensorPose, ParamsSensorPose, 7);
~SensorPose() override;
// double getStdP() const;
// double getStdO() const;
}; template <unsigned int DIM>
inline Eigen::MatrixXd SensorPose<DIM>::computeNoiseCov(const Eigen::VectorXd& _data) const
// inline double SensorPose::getStdP() const {
// { return getStdNoise().cwiseAbs2().asDiagonal();
// return std_p_; }
// }
// inline double SensorPose::getStdO() const
// {
// return std_o_;
// }
// inline Matrix6d SensorPose::computeDataCov() const typedef SensorPose<2> SensorPose2d;
// { typedef SensorPose<3> SensorPose3d;
// return (Vector6d() << std_p_*std_p_, std_p_*std_p_, std_p_*std_p_, std_o_*std_o_, std_o_*std_o_, std_o_*std_o_).finished().asDiagonal(); WOLF_DECLARED_PTR_TYPEDEFS(SensorPose2d);
// } WOLF_DECLARED_PTR_TYPEDEFS(SensorPose3d);
} /* namespace wolf */ } /* namespace wolf */
#endif /* SRC_SENSOR_POSE_H_ */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/** // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file factory_solver.h #pragma once
*
* Created on: Dec 17, 2018
* \author: jcasals
*/
#ifndef FACTORY_SOLVER_H_
#define FACTORY_SOLVER_H_
namespace wolf
{
class SensorBase;
struct ParamsSensorBase;
}
// wolf // wolf
#include "core/common/factory.h" #include "core/common/factory.h"
#include "core/solver/solver_manager.h"
#include "yaml-cpp/yaml.h"
namespace wolf namespace wolf
{ {
/** \brief Solver factory class /** \brief Solver factory class
* *
*/ */
typedef Factory<SolverManager, typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const YAML::Node&, const std::vector<std::string>&>
const ProblemPtr&, FactorySolverNode;
const ParamsServer&> FactorySolver;
template<> template <>
inline std::string FactorySolver::getClass() const inline std::string FactorySolverNode::getClass() const
{ {
return "FactorySolver"; return "FactorySolverNode";
} }
#define WOLF_REGISTER_SOLVER(SolverType) \ typedef Factory<std::shared_ptr<SolverManager>, const ProblemPtr&, const std::string&, const std::vector<std::string>&>
namespace{ const bool WOLF_UNUSED SolverType##Registered = \ FactorySolverFile;
wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
} /* namespace wolf */ template <>
inline std::string FactorySolverFile::getClass() const
{
return "FactorySolverFile";
}
#endif /* SENSOR_FACTORY_H_ */ #define WOLF_REGISTER_SOLVER(SolverType) \
namespace \
{ \
const bool WOLF_UNUSED SolverType##Registered = \
wolf::FactorySolverNode::registerCreator(#SolverType, SolverType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED SolverType##RegisteredYaml = \
wolf::FactorySolverFile::registerCreator(#SolverType, SolverType::create); \
}
} /* namespace wolf */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
#ifndef _WOLF_SOLVER_MANAGER_H_ // along with this program. If not, see <http://www.gnu.org/licenses/>.
#define _WOLF_SOLVER_MANAGER_H_ #pragma once
//wolf includes // wolf includes
#include "core/common/wolf.h" #include "core/common/wolf.h"
#include "core/state_block/state_block.h" #include "core/state_block/state_block.h"
#include "core/factor/factor_base.h" #include "core/factor/factor_base.h"
#include "core/common/params_base.h" #include "core/solver/factory_solver.h"
#include "core/common/profiling_unit.h"
namespace wolf {
namespace wolf
{
WOLF_PTR_TYPEDEFS(SolverManager) WOLF_PTR_TYPEDEFS(SolverManager)
WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
/* /*
* Macro for defining Autoconf solver creator for WOLF's high level API. * Macro for defining Autoconf solver creator for WOLF's high level API.
* *
* Place a call to this macro inside your class declaration (in the solver_manager_class.h file), * Place a call to this macro inside your class declaration (in the solver_class.h file),
* preferably just after the constructors. * preferably just after the constructors.
* *
* In order to use this macro, the derived solver manager class, SolverManagerClass, * In order to use this macro, the derived solver manager class, SolverClass,
* must have a constructor available with the API: * must have a constructor available with the API:
* *
* SolverManagerClass(const ProblemPtr& wolf_problem, * SolverClass(const ProblemPtr& wolf_problem,
* const ParamsSolverClassPtr _params); * const YAML::Node _params);
*/ */
#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \ #define WOLF_SOLVER_CREATE(SolverClass) \
static SolverManagerPtr create(const ProblemPtr& _problem, \ static SolverManagerPtr create(const ProblemPtr& _problem, \
const ParamsServer& _server) \ const YAML::Node& _input_node, \
{ \ const std::vector<std::string>& _folders_schema = {}) \
auto params = std::make_shared<ParamsSolverClass>(#SolverClass, _server); \ { \
return std::make_shared<SolverClass>(_problem, params); \ if (not _folders_schema.empty()) \
} \ { \
static SolverManagerPtr create(const ProblemPtr& _problem, \ auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
const ParamsSolverPtr _params) \ server.setYaml(_input_node); \
{ \ if (not server.applySchema(#SolverClass)) \
auto params = std::static_pointer_cast<ParamsSolverClass>(_params); \ { \
return std::make_shared<SolverClass>(_problem, params); \ WOLF_ERROR(server.getLog()); \
} \ return nullptr; \
} \
struct ParamsSolver; } \
return std::make_shared<SolverClass>(_problem, _input_node); \
} \
static SolverManagerPtr create(const ProblemPtr& _problem, \
const std::string& _yaml_filepath, \
const std::vector<std::string>& _folders_schema) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema, _yaml_filepath); \
if (not server.applySchema(#SolverClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(_problem, server.getNode(), {}); \
}
/** /**
* \brief Solver manager for WOLF * \brief Solver manager for WOLF
*/ */
class SolverManager class SolverManager
{ {
public: public:
/** \brief Enumeration of covariance blocks to be computed
/** \brief Enumeration of covariance blocks to be computed */
* enum class CovarianceBlocksToBeComputed : std::size_t
* Enumeration of covariance blocks to be computed {
* ALL = 0, ///< All blocks and all cross-covariances
*/ ALL_MARGINALS = 1, ///< All marginals
enum class CovarianceBlocksToBeComputed : std::size_t ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current
{ ///< robot and all landmarks
ALL = 0, ///< All blocks and all cross-covariances GAUSS = 3, ///< GAUSS: last frame P and V
ALL_MARGINALS = 1, ///< All marginals GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
ROBOT_LANDMARKS = 2, ///< marginals of landmarks and current robot pose plus cross covariances of current robot and all landmarks GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
GAUSS = 3, ///< GAUSS: last frame P and V };
GAUSS_TUCAN = 4, ///< GAUSS: last frame P, O, V and T
GAUSS_TUCAN_ONLY_POSITION = 5 ///< GAUSS: last frame P and T
};
/**
* \brief Enumeration for the verbosity of the solver report.
*/
enum class ReportVerbosity : std::size_t
{
QUIET = 0,
BRIEF,
FULL
};
// PROFILING
unsigned int n_solve_, n_cov_;
std::chrono::microseconds acc_duration_total_;
std::chrono::microseconds acc_duration_solver_;
std::chrono::microseconds acc_duration_update_;
std::chrono::microseconds acc_duration_state_;
std::chrono::microseconds acc_duration_cov_;
std::chrono::microseconds max_duration_total_;
std::chrono::microseconds max_duration_solver_;
std::chrono::microseconds max_duration_update_;
std::chrono::microseconds max_duration_state_;
std::chrono::microseconds max_duration_cov_;
void printProfiling(std::ostream& stream = std::cout) const;
protected:
ProblemPtr wolf_problem_;
ParamsSolverPtr params_;
public:
/**
* \brief Constructor with default params_
*/
SolverManager(const ProblemPtr& _problem);
/**
* \brief Constructor with given params_
*/
SolverManager(const ProblemPtr& _problem,
const ParamsSolverPtr& _params);
virtual ~SolverManager();
/**
* \brief Solves with the verbosity defined in params_
*/
std::string solve();
/**
* \brief Solves with a given verbosity
*/
std::string solve(const ReportVerbosity report_level);
virtual bool computeCovariances() final;
virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
/**
* \brief Updates solver's problem according to the wolf_problem
*/
virtual void update();
ProblemPtr getProblem();
virtual ParamsSolverPtr getParams() const;
double getPeriod() const;
double getCovPeriod() const;
ReportVerbosity getVerbosity() const;
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
virtual bool isStateBlockFixed(const StateBlockPtr& st) final;
virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) final;
virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
virtual int numFactors() const final;
virtual int numStateBlocks() const final;
virtual int numStateBlocksFloating() const final;
virtual int numFactorsDerived() const = 0;
virtual int numStateBlocksDerived() const = 0;
virtual bool check(std::string prefix="") const final; /** \brief Enumeration for the verbosity of the solver report.
*/
enum class ReportVerbosity : std::size_t
{
QUIET = 0,
BRIEF = 1,
FULL = 2
};
protected: // PROFILING
void printProfiling(std::ostream& stream = std::cout) const;
protected:
ProblemPtr wolf_problem_;
ProfilingUnit profiling_total_, profiling_solve_, profiling_update_, profiling_state_, profiling_cov_;
YAML::Node params_;
public:
/**
* \brief Constructor with default params_
*/
SolverManager(const ProblemPtr& _problem);
/**
* \brief Constructor with given params_
*/
SolverManager(const ProblemPtr& _problem, const YAML::Node& _params);
static SolverManagerPtr autoSetup(const ProblemPtr& _problem,
const std::string& _input_yaml_file,
std::vector<std::string> _primary_schema_folders,
bool _skip_yaml_validation = false);
static SolverManagerPtr autoSetup(const ProblemPtr& _problem,
YAML::Node _param_node,
std::vector<std::string> _primary_schema_folders,
bool _skip_yaml_validation = false);
virtual ~SolverManager();
/**
* \brief Solves with the verbosity defined in params_
*/
std::string solve();
/**
* \brief Solves with a given verbosity
*/
std::string solve(const ReportVerbosity report_level);
virtual bool computeCovariances() final;
virtual bool computeCovariances(const CovarianceBlocksToBeComputed blocks) final;
virtual bool computeCovariances(const std::vector<StateBlockPtr>& st_list) final;
/**
* \brief Updates solver's problem according to the wolf_problem
*/
virtual void update();
ProblemPtr getProblem();
YAML::Node getParams() const;
bool getComputeCov() const;
double getPeriod() const;
double getCovPeriod() const;
ReportVerbosity getVerbosity() const;
virtual bool isStateBlockRegistered(const StateBlockPtr& state_ptr) const final;
virtual bool isStateBlockFloating(const StateBlockPtr& state_ptr) const final;
virtual bool isStateBlockFixed(const StateBlockPtr& st) const final;
virtual bool isFactorRegistered(const FactorBasePtr& fac_ptr) const final;
virtual bool hasThisLocalParametrization(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) const final;
virtual bool hasLocalParametrization(const StateBlockPtr& st) const final;
virtual int numFactors() const final;
virtual int numStateBlocks() const final;
virtual int numStateBlocksFloating() const final;
virtual int numFactorsDerived() const = 0;
virtual int numStateBlocksDerived() const = 0;
virtual bool check(std::string prefix = "") const final;
protected:
std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_;
std::set<FactorBasePtr> factors_;
std::set<StateBlockPtr> floating_state_blocks_;
virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const;
double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
private:
// SolverManager functions
virtual void addFactor(const FactorBasePtr& fac_ptr) final;
virtual void removeFactor(const FactorBasePtr& fac_ptr) final;
virtual void addStateBlock(const StateBlockPtr& state_ptr) final;
virtual void removeStateBlock(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final;
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
// Pure virtual functions to be derived
protected:
virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_; virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
std::map<StateBlockPtr, FactorBasePtrList> state_blocks_2_factors_; virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
std::set<FactorBasePtr> factors_; virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) const = 0;
std::set<StateBlockPtr> floating_state_blocks_; virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) const = 0;
virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr); virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
const double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr) const; virtual bool checkDerived(std::string prefix = "") const = 0;
double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
private: public:
// SolverManager functions virtual bool converged() const = 0;
virtual void addFactor(const FactorBasePtr& fac_ptr) final; virtual bool failed() const = 0;
virtual void removeFactor(const FactorBasePtr& fac_ptr) final; virtual bool wasStopped() const = 0;
virtual void addStateBlock(const StateBlockPtr& state_ptr) final; virtual unsigned int iterations() const = 0;
virtual void removeStateBlock(const StateBlockPtr& state_ptr) final; virtual double initialCost() const = 0;
virtual void updateStateBlockState(const StateBlockPtr& state_ptr) final; virtual double finalCost() const = 0;
virtual void updateStateBlockStatus(const StateBlockPtr& state_ptr) final; virtual double totalTime() const = 0;
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) final;
// Derived virtual functions
protected:
virtual std::string solveDerived(const ReportVerbosity report_level) = 0;
virtual bool computeCovariancesDerived(const CovarianceBlocksToBeComputed blocks) = 0;
virtual bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) = 0;
virtual void addFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void removeFactorDerived(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void removeStateBlockDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) = 0;
virtual void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) = 0;
virtual bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const = 0;
virtual bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const = 0;
virtual bool isStateBlockFixedDerived(const StateBlockPtr& st) = 0;
virtual bool hasLocalParametrizationDerived(const StateBlockPtr& st) const = 0;
virtual bool hasThisLocalParametrizationDerived(const StateBlockPtr& st,
const LocalParametrizationBasePtr& local_param) = 0;
virtual void printProfilingDerived(std::ostream& stream = std::cout) const = 0;
virtual bool checkDerived(std::string prefix="") const = 0;
public:
virtual bool hasConverged() = 0;
virtual bool wasStopped() = 0;
virtual unsigned int iterations() = 0;
virtual double initialCost() = 0;
virtual double finalCost() = 0;
virtual double totalTime() = 0;
protected:
// PARAMS
double period_;
ReportVerbosity verbose_;
bool compute_cov_;
CovarianceBlocksToBeComputed cov_enum_;
double cov_period_;
}; };
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity) } // namespace wolf
struct ParamsSolver: public ParamsBase
{
std::string prefix = "solver/";
double period = 0.0;
SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
bool compute_cov = false;
SolverManager::CovarianceBlocksToBeComputed cov_enum = SolverManager::CovarianceBlocksToBeComputed::ROBOT_LANDMARKS;
double cov_period = 1.0;
ParamsSolver() = default;
ParamsSolver(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
period = _server.getParam<double>(prefix + "period");
verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
compute_cov = _server.getParam<bool>(prefix + "compute_cov");
if (compute_cov)
{
cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_server.getParam<int>(prefix + "cov_enum");
cov_period = _server.getParam<double>(prefix + "cov_period");
}
}
std::string print() const override
{
return "period: " + std::to_string(period) + "\n" +
"verbose: " + std::to_string((int)verbose) + "\n" +
"compute_cov: " + std::to_string(compute_cov) + "\n" +
"cov_enum: " + std::to_string((int)cov_enum) + "\n" +
"cov_period: " + std::to_string(cov_period) + "\n";
}
~ParamsSolver() override = default;
};
} // namespace wolf
#endif /* _WOLF_SOLVER_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)
// 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--------
/*
* ccolamd_ordering.h
*
* Created on: Jun 12, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
#define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
//std includes
#include <iostream>
// Eigen includes
#include <Eigen/OrderingMethods>
#include <Eigen/CholmodSupport>
#include <Eigen/SparseLU>
// ccolamd
#include "ccolamd.h"
namespace Eigen{
template<typename Index>
class CCOLAMDOrdering
{
public:
typedef PermutationMatrix<Dynamic, Dynamic, Index> PermutationType;
typedef Matrix<Index, Dynamic, 1> IndexVector;
template<typename MatrixType>
void operator()(const MatrixType& mat, PermutationType& perm, Index* cmember = nullptr)
{
Index m = mat.rows();
Index n = mat.cols();
Index nnz = mat.nonZeros();
// Get the recommended value of Alen to be used by colamd
Index Alen = ccolamd_recommended(nnz, m, n);
// Set the default parameters
double knobs[CCOLAMD_KNOBS];
Index stats[CCOLAMD_STATS];
ccolamd_set_defaults(knobs);
IndexVector p(n + 1), A(Alen);
for (Index i = 0; i <= n; i++)
p(i) = mat.outerIndex()[i];
for (Index i = 0; i < nnz; i++)
A(i) = mat.innerIndex()[i];
// std::cout << "p = " << std::endl << p.transpose() << std::endl;
// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
// Call CColamd routine to compute the ordering
Index info = compute_ccolamd(m, n, Alen, A.data(), p.data(), knobs, stats, cmember);
if (!info)
assert(info && "CCOLAMD failed ");
perm.resize(n);
for (Index i = 0; i < n; i++)
perm.indices()(p(i)) = i;
}
private:
int compute_ccolamd(int &m, int &n, int &Alen, int* A, int* p, double* knobs, int* stats, int* cmember)
{
int info = ccolamd(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_report (stats) ;
return info;
}
long int compute_ccolamd(long int &m, long int &n, long int &Alen, long int* A, long int* p, double* knobs, long int* stats, long int* cmember)
{
long int info = ccolamd_l(m, n, Alen, A, p, knobs, stats, cmember);
//ccolamd_l_report (stats) ;
return info;
}
};
}
#endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_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)
// 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--------
/*
* cost_function_base.h
*
* Created on: Jun 25, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
#include "core/common/wolf.h"
#include <Eigen/StdVector>
class CostFunctionBase
{
protected:
unsigned int n_blocks_;
Eigen::MatrixXd J_0_;
Eigen::MatrixXd J_1_;
Eigen::MatrixXd J_2_;
Eigen::MatrixXd J_3_;
Eigen::MatrixXd J_4_;
Eigen::MatrixXd J_5_;
Eigen::MatrixXd J_6_;
Eigen::MatrixXd J_7_;
Eigen::MatrixXd J_8_;
Eigen::MatrixXd J_9_;
Eigen::VectorXd residual_;
std::vector<Eigen::MatrixXd*> jacobians_;
std::vector<unsigned int> block_sizes_;
public:
CostFunctionBase(const unsigned int &_measurement_size,
const unsigned int &_block_0_size,
const unsigned int &_block_1_size,
const unsigned int &_block_2_size,
const unsigned int &_block_3_size,
const unsigned int &_block_4_size,
const unsigned int &_block_5_size,
const unsigned int &_block_6_size,
const unsigned int &_block_7_size,
const unsigned int &_block_8_size,
const unsigned int &_block_9_size) :
n_blocks_(10),
J_0_(_measurement_size, _block_0_size),
J_1_(_measurement_size, _block_1_size),
J_2_(_measurement_size, _block_2_size),
J_3_(_measurement_size, _block_3_size),
J_4_(_measurement_size, _block_4_size),
J_5_(_measurement_size, _block_5_size),
J_6_(_measurement_size, _block_6_size),
J_7_(_measurement_size, _block_7_size),
J_8_(_measurement_size, _block_8_size),
J_9_(_measurement_size, _block_9_size),
residual_(_measurement_size),
jacobians_({&J_0_,&J_1_,&J_2_,&J_3_,&J_4_,&J_5_,&J_6_,&J_7_,&J_8_,&J_9_}),
block_sizes_({_block_0_size, _block_1_size, _block_2_size, _block_3_size, _block_4_size, _block_5_size, _block_6_size, _block_7_size, _block_8_size, _block_9_size})
{
for (unsigned int i = 1; i<n_blocks_; i++)
{
if (block_sizes_.at(i) == 0)
{
n_blocks_ = i;
jacobians_.resize(n_blocks_);
block_sizes_.resize(n_blocks_);
break;
}
}
}
virtual ~CostFunctionBase()
{
}
virtual void evaluateResidualJacobians() = 0;
void getResidual(Eigen::VectorXd &residual)
{
residual.resize(residual_.size());
residual = residual_;
}
std::vector<Eigen::MatrixXd*> getJacobians()
{
return jacobians_;
}
void getJacobians(std::vector<Eigen::MatrixXd>& jacobians)
{
jacobians.resize(n_blocks_);
for (unsigned int i = 0; i<n_blocks_; i++)
jacobians.at(i) = (*jacobians_.at(i));
}
};
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_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)
// 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 TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
//wolf includes
#include "core/common/wolf.h"
#include "cost_function_sparse_base.h"
// CERES JET
#include "ceres/jet.h"
namespace wolf
{
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
class CostFunctionSparse : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>(_factor_ptr)
{
}
void callFunctor()
{
// if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE > 0)
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
// (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
// else
// assert("Wrong combination of zero sized blocks");
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
}
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE>
class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
0>
{
public:
CostFunctionSparse(FactorT* _factor_ptr) :
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
0>(_factor_ptr)
{
}
void callFunctor()
{
(*this->factor_ptr_)(this->jets_0_,this->residuals_jet_);
}
};
} //namespace wolf
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_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)
// 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--------
/*
* cost_function_sparse.h
*
* Created on: Jun 25, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
//wolf includes
#include "core/common/wolf.h"
#include "cost_function_base.h"
// CERES JET
#include "ceres/jet.h"
namespace wolf
{
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE = 0,
unsigned int BLOCK_2_SIZE = 0,
unsigned int BLOCK_3_SIZE = 0,
unsigned int BLOCK_4_SIZE = 0,
unsigned int BLOCK_5_SIZE = 0,
unsigned int BLOCK_6_SIZE = 0,
unsigned int BLOCK_7_SIZE = 0,
unsigned int BLOCK_8_SIZE = 0,
unsigned int BLOCK_9_SIZE = 0>
class CostFunctionSparseBase : CostFunctionBase
{
typedef ceres::Jet<double, BLOCK_0_SIZE +
BLOCK_1_SIZE +
BLOCK_2_SIZE +
BLOCK_3_SIZE +
BLOCK_4_SIZE +
BLOCK_5_SIZE +
BLOCK_6_SIZE +
BLOCK_7_SIZE +
BLOCK_8_SIZE +
BLOCK_9_SIZE> WolfJet;
protected:
FactorT* factor_ptr_;
WolfJet jets_0_[BLOCK_0_SIZE];
WolfJet jets_1_[BLOCK_1_SIZE];
WolfJet jets_2_[BLOCK_2_SIZE];
WolfJet jets_3_[BLOCK_3_SIZE];
WolfJet jets_4_[BLOCK_4_SIZE];
WolfJet jets_5_[BLOCK_5_SIZE];
WolfJet jets_6_[BLOCK_6_SIZE];
WolfJet jets_7_[BLOCK_7_SIZE];
WolfJet jets_8_[BLOCK_8_SIZE];
WolfJet jets_9_[BLOCK_9_SIZE];
WolfJet residuals_jet_[MEASUREMENT_SIZE];
public:
/** \brief Constructor with factor pointer
*
* Constructor with factor pointer
*
*/
CostFunctionSparseBase(FactorT* _factor_ptr);
/** \brief Default destructor
*
* Default destructor
*
*/
virtual ~CostFunctionSparseBase();
/** \brief Evaluate residuals and jacobians of the factor in the current x
*
* Evaluate residuals and jacobians of the factor in the current x
*
*/
virtual void evaluateResidualJacobians();
protected:
/** \brief Calls the functor of the factor evaluating jets
*
* Calls the functor of the factor evaluating jets
*
*/
virtual void callFunctor() = 0;
/** \brief Initialize the infinitesimal part of jets
*
* Initialize the infinitesimal part of jets with zeros and ones
*
*/
void initializeJets();
/** \brief Gets the evaluation point
*
* Gets the evaluation point from the state
*
*/
void evaluateX();
};
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) :
CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE),
factor_ptr_(_factor_ptr)
{
initializeJets();
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>::~CostFunctionSparseBase()
{
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>::evaluateResidualJacobians()
{
evaluateX();
callFunctor();
// fill the jacobian matrices
int jacobian_location = 0;
for (int i = 0; i<n_blocks_; i++)
{
for (int row = 0; row < MEASUREMENT_SIZE; row++)
jacobians_.at(i)->row(row) = residuals_jet_[row].v.segment(jacobian_location, block_sizes_.at(i));
jacobian_location += block_sizes_.at(i);
std::cout << "filled jacobian " << i << ":" << std::endl << (*jacobians_.at(i)) << std::endl;
}
// fill the residual vector
for (int i = 0; i < MEASUREMENT_SIZE; i++)
residual_(i) = residuals_jet_[i].a;
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>::initializeJets()
{
int last_jet_idx = 0;
// JET 0
for (int i = 0; i < BLOCK_0_SIZE; i++)
jets_0_[i] = WolfJet(0, last_jet_idx++);
// JET 1
for (int i = 0; i < BLOCK_1_SIZE; i++)
jets_1_[i] = WolfJet(0, last_jet_idx++);
// JET 2
for (int i = 0; i < BLOCK_2_SIZE; i++)
jets_2_[i] = WolfJet(0, last_jet_idx++);
// JET 3
for (int i = 0; i < BLOCK_3_SIZE; i++)
jets_3_[i] = WolfJet(0, last_jet_idx++);
// JET 4
for (int i = 0; i < BLOCK_4_SIZE; i++)
jets_4_[i] = WolfJet(0, last_jet_idx++);
// JET 5
for (int i = 0; i < BLOCK_5_SIZE; i++)
jets_5_[i] = WolfJet(0, last_jet_idx++);
// JET 6
for (int i = 0; i < BLOCK_6_SIZE; i++)
jets_6_[i] = WolfJet(0, last_jet_idx++);
// JET 7
for (int i = 0; i < BLOCK_7_SIZE; i++)
jets_7_[i] = WolfJet(0, last_jet_idx++);
// JET 8
for (int i = 0; i < BLOCK_8_SIZE; i++)
jets_8_[i] = WolfJet(0, last_jet_idx++);
// JET 9
for (int i = 0; i < BLOCK_9_SIZE; i++)
jets_9_[i] = WolfJet(0, last_jet_idx++);
}
template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
BLOCK_9_SIZE>::evaluateX()
{
// JET 0
for (int i = 0; i < BLOCK_0_SIZE; i++)
jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0)+i);
// JET 1
for (int i = 0; i < BLOCK_1_SIZE; i++)
jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1)+i);
// JET 2
for (int i = 0; i < BLOCK_2_SIZE; i++)
jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2)+i);
// JET 3
for (int i = 0; i < BLOCK_3_SIZE; i++)
jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3)+i);
// JET 4
for (int i = 0; i < BLOCK_4_SIZE; i++)
jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4)+i);
// JET 5
for (int i = 0; i < BLOCK_5_SIZE; i++)
jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5)+i);
// JET 6
for (int i = 0; i < BLOCK_6_SIZE; i++)
jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6)+i);
// JET 7
for (int i = 0; i < BLOCK_7_SIZE; i++)
jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7)+i);
// JET 8
for (int i = 0; i < BLOCK_8_SIZE; i++)
jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8)+i);
// JET 9
for (int i = 0; i < BLOCK_9_SIZE; i++)
jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i);
}
} // wolf namespace
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_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)
// 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_solver.h
*
* Created on: Jul 2, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_QR_SOLVER_H_
#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
//std includes
#include <core/factor/factor_odom_2d.h>
#include <iostream>
#include <ctime>
//Wolf includes
#include "core/state_block/state_block.h"
#include "../factor_sparse.h"
#include "core/factor/factor_corner_2d.h"
#include "core/factor/factor_container.h"
#include "core/solver_suitesparse/sparse_utils.h"
// wolf solver
#include "solver/ccolamd_ordering.h"
#include "solver/cost_function_sparse.h"
// eigen includes
#include <Eigen/OrderingMethods>
#include <Eigen/SparseQR>
#include <Eigen/StdVector>
#include "core/factor/factor_pose_2d.h"
namespace wolf
{
class SolverQR
{
protected:
ProblemPtr problem_ptr_;
Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::NaturalOrdering<int>> solver_;
Eigen::SparseMatrix<double> A_, R_;
Eigen::VectorXd b_, x_incr_;
std::vector<StateBlockPtr> nodes_;
std::vector<FactorBasePtr> factors_;
std::vector<CostFunctionBasePtr> cost_functions_;
// ordering
Eigen::SparseMatrix<int> A_nodes_;
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> acc_node_permutation_;
std::map<double*, unsigned int> id_2_idx_;
Eigen::CCOLAMDOrdering<int> orderer_;
Eigen::VectorXi node_ordering_restrictions_;
Eigen::ArrayXi node_locations_;
std::vector<unsigned int> factor_locations_;
unsigned int n_new_factors_;
// time
clock_t t_ordering_, t_solving_, t_managing_;
double time_ordering_, time_solving_, time_managing_;
public:
SolverQR(ProblemPtr problem_ptr_) :
problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_factors_(
0), time_ordering_(0), time_solving_(0), time_managing_(0)
{
node_locations_.resize(0);
factor_locations_.resize(0);
}
virtual ~SolverQR()
{
}
void update()
{
// UPDATE STATE BLOCKS
while (!problem_ptr_->getStateBlockNotificationList().empty())
{
switch (problem_ptr_->getStateBlockNotificationList().front().notification_)
{
case ADD:
{
addStateBlock(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
break;
}
case UPDATE:
{
updateStateBlockStatus(problem_ptr_->getStateBlockNotificationList().front().state_block_ptr_);
break;
}
case REMOVE:
{
// TODO removeStateBlock((double *)(problem_ptr_->getStateBlockNotificationList().front().scalar_ptr_));
break;
}
default:
throw std::runtime_error("SolverQR::update: State Block notification must be ADD, UPATE or REMOVE.");
}
problem_ptr_->getStateBlockNotificationList().pop_front();
}
// UPDATE FACTORS
while (!problem_ptr_->getFactorNotificationList().empty())
{
switch (problem_ptr_->getFactorNotificationList().front().notification_)
{
case ADD:
{
addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_);
break;
}
case REMOVE:
{
// TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_);
break;
}
default:
throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE.");
}
problem_ptr_->getFactorNotificationList().pop_front();
}
}
void addStateBlock(StateBlockPtr _state_ptr)
{
t_managing_ = clock();
std::cout << "adding state unit " << _state_ptr->get() << std::endl;
if (!_state_ptr->isFixed())
{
nodes_.push_back(_state_ptr);
id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
// Resize accumulated permutations
augmentPermutation(acc_node_permutation_, nNodes());
// Resize state
x_incr_.conservativeResize(x_incr_.size() + _state_ptr->getSize());
// Resize problem
A_.conservativeResize(A_.rows(), A_.cols() + _state_ptr->getSize());
R_.conservativeResize(R_.cols() + _state_ptr->getSize(), R_.cols() + _state_ptr->getSize());
}
time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
}
void updateStateBlockStatus(StateBlockPtr _state_ptr)
{
//TODO
}
void addFactor(FactorBasePtr _factor_ptr)
{
std::cout << "adding factor " << _factor_ptr->id() << std::endl;
t_managing_ = clock();
factors_.push_back(_factor_ptr);
cost_functions_.push_back(createCostFunction(_factor_ptr));
unsigned int meas_dim = _factor_ptr->getSize();
std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size());
Eigen::VectorXd error(meas_dim);
cost_functions_.back()->evaluateResidualJacobians();
cost_functions_.back()->getResidual(error);
cost_functions_.back()->getJacobians(jacobians);
std::vector<unsigned int> idxs;
for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
n_new_factors_++;
factor_locations_.push_back(A_.rows());
// Resize problem
A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
b_.conservativeResize(b_.size() + meas_dim);
A_nodes_.conservativeResize(factors_.size(), nNodes());
// ADD MEASUREMENTS
for (unsigned int j = 0; j < idxs.size(); j++)
{
assert((unsigned int )(acc_node_permutation_.indices()(idxs.at(j))) == nodeOrder(idxs.at(j)));
assert(jacobians.at(j).cols() == nodeDim(idxs.at(j)));
assert(jacobians.at(j).rows() == meas_dim);
addSparseBlock(jacobians.at(j), A_, A_.rows() - meas_dim, nodeLocation(idxs.at(j)));
A_nodes_.coeffRef(A_nodes_.rows() - 1, nodeOrder(idxs.at(j))) = 1;
}
// error
b_.tail(meas_dim) = error;
time_managing_ += ((double)clock() - t_managing_) / CLOCKS_PER_SEC;
}
void ordering(const int & _first_ordered_idx)
{
std::cout << "ordering from idx " << _first_ordered_idx << std::endl;
t_ordering_ = clock();
// full problem ordering
if (_first_ordered_idx == -1)
{
// ordering ordering factors
node_ordering_restrictions_.resize(nNodes());
node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
A_nodes_.makeCompressed();
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(nNodes());
orderer_(A_nodes_, incr_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation(A_.cols());
nodePermutation2VariablesPermutation(incr_permutation_nodes, incr_permutation);
// apply partial_ordering orderings
A_nodes_ = (A_nodes_ * incr_permutation_nodes.transpose()).sparseView();
A_ = (A_ * incr_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulatePermutation(incr_permutation_nodes);
}
// partial ordering
else
{
unsigned int first_ordered_node = nodeOrder(_first_ordered_idx); //nodes_.at(_first_ordered_idx).order;
unsigned int ordered_nodes = nNodes() - first_ordered_node;
if (ordered_nodes > 2) // only reordering when involved nodes in the measurement are not the two last ones
{
// SUBPROBLEM ORDERING (from first node variable to last one)
//std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
// _partial_ordering ordering_ factors
node_ordering_restrictions_.resize(ordered_nodes);
node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
// computing nodes partial ordering_
sub_A_nodes_.makeCompressed();
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation_nodes(ordered_nodes);
orderer_(sub_A_nodes_, partial_permutation_nodes, node_ordering_restrictions_.data());
// node ordering to variable ordering
unsigned int ordered_variables = A_.cols() - nodeLocation(_first_ordered_idx); //nodes_.at(_first_ordered_idx).location;
// std::cout << "first_ordered_node " << first_ordered_node << std::endl;
// std::cout << "A_.cols() " << A_.cols() << std::endl;
// std::cout << "nodes_.at(_first_ordered_idx).location " << nodes_.at(_first_ordered_idx).location << std::endl;
// std::cout << "ordered_variables " << ordered_variables << std::endl;
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> partial_permutation(ordered_variables);
nodePermutation2VariablesPermutation(partial_permutation_nodes, partial_permutation);
// apply partial_ordering orderings
A_nodes_.rightCols(ordered_nodes) = (A_nodes_.rightCols(ordered_nodes)
* partial_permutation_nodes.transpose()).sparseView();
A_.rightCols(ordered_variables) =
(A_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
R_.rightCols(ordered_variables) =
(R_.rightCols(ordered_variables) * partial_permutation.transpose()).sparseView();
// ACCUMULATING PERMUTATIONS
accumulatePermutation(partial_permutation_nodes);
}
}
time_ordering_ += ((double)clock() - t_ordering_) / CLOCKS_PER_SEC;
}
unsigned int findFirstOrderedNode()
{
unsigned int first_ordered_node = nNodes();
unsigned int first_ordered_idx;
for (unsigned int i = 0; i < n_new_factors_; i++)
{
FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i);
std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id()
<< std::endl;
for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++)
{
if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
{
unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
//std::cout << "estimated idx " << idx << std::endl;
//std::cout << "node_order(idx) " << node_order(idx) << std::endl;
//std::cout << "first_ordered_node " << first_ordered_node << std::endl;
if (first_ordered_node > nodeOrder(idx)) //nodes_.at(idx).order)
{
first_ordered_idx = idx;
//std::cout << "first_ordered_idx " << first_ordered_idx << std::endl;
first_ordered_node = nodeOrder(idx);
//std::cout << "first_ordered_node " << first_ordered_node << std::endl;
}
}
}
}
//std::cout << "found first_ordered_node " << first_ordered_node << std::endl;
//std::cout << "found first_ordered_idx " << first_ordered_idx << std::endl;
return first_ordered_idx;
}
bool solve(const unsigned int mode)
{
if (n_new_factors_ == 0)
return 1;
std::cout << "solving mode " << mode << std::endl;
bool batch, order;
unsigned int first_ordered_idx;
switch (mode)
{
case 0:
{
batch = true;
order = false;
break;
}
case 1:
{
batch = true;
order = (nNodes() > 1);
break;
}
case 2:
{
first_ordered_idx = findFirstOrderedNode();
batch = (nodeOrder(first_ordered_idx) == 0);
order = (nNodes() > 1);
}
}
// BATCH
if (batch)
{
// REORDER
if (order)
ordering(-1);
//printProblem();
// SOLVE
t_solving_ = clock();
A_.makeCompressed();
solver_.compute(A_);
if (solver_.info() != Eigen::Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_ = solver_.solve(-b_);
R_ = solver_.matrixR();
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.cols(), R_.cols()) * R_ << std::endl;
time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
}
// INCREMENTAL
else
{
// REORDER SUBPROBLEM
ordering(first_ordered_idx);
//printProblem();
// SOLVE ORDERED SUBPROBLEM
t_solving_ = clock();
A_nodes_.makeCompressed();
A_.makeCompressed();
// finding measurements block
Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
//std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
//std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
unsigned int first_ordered_measurement =
measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
Eigen::SparseMatrix<double> A_partial = A_.bottomRightCorner(ordered_measurements, ordered_variables);
solver_.compute(A_partial);
if (solver_.info() != Eigen::Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
//std::cout << "R new" << std::endl << MatrixXd::Identity(A_partial.cols(), A_partial.cols()) * solver_.matrixR() << std::endl;
x_incr_.tail(ordered_variables) = solver_.solve(-b_.tail(ordered_measurements));
// store new part of R
eraseSparseBlock(R_, unordered_variables, unordered_variables, ordered_variables, ordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
addSparseBlock(solver_.matrixR(), R_, unordered_variables, unordered_variables);
//std::cout << "R" << std::endl << MatrixXd::Identity(R_.rows(), R_.rows()) * R_ << std::endl;
R_.makeCompressed();
// solving not ordered subproblem
if (unordered_variables > 0)
{
//std::cout << "--------------------- solving unordered part" << std::endl;
Eigen::SparseMatrix<double> R1 = R_.topLeftCorner(unordered_variables, unordered_variables);
//std::cout << "R1" << std::endl << MatrixXd::Identity(R1.rows(), R1.rows()) * R1 << std::endl;
Eigen::SparseMatrix<double> R2 = R_.topRightCorner(unordered_variables, ordered_variables);
//std::cout << "R2" << std::endl << MatrixXd::Identity(R2.rows(), R2.rows()) * R2 << std::endl;
solver_.compute(R1);
if (solver_.info() != Eigen::Success)
{
std::cout << "decomposition failed" << std::endl;
return 0;
}
x_incr_.head(unordered_variables) = solver_.solve(
-b_.head(unordered_variables) + R2 * x_incr_.tail(ordered_variables));
}
}
// UPDATE X VALUE
for (unsigned int i = 0; i < nodes_.size(); i++)
{
Eigen::Map < Eigen::VectorXd > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
b_.setZero();
time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
n_new_factors_ = 0;
return 1;
}
void nodePermutation2VariablesPermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm_variables)
{
//std::cout << "perm_nodes: " << _perm_nodes.indices().transpose() << std::endl;
nodePermutation2nodeLocations(_perm_nodes, node_locations_);
//std::cout << "locations: " << locations.transpose() << std::endl;
//std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
unsigned int last_idx = 0;
for (unsigned int i = 0; i < node_locations_.size(); i++)
{
perm_variables.indices().segment(last_idx, nodeDim(i)) = Eigen::VectorXi::LinSpaced(
nodeDim(i), node_locations_(i), node_locations_(i) + nodeDim(i) - 1);
last_idx += nodeDim(i);
//std::cout << i << " perm_variables: " << perm_variables.indices().transpose() << std::endl;
}
//std::cout << "perm_variables: " << perm_variables.indices().transpose() << std::endl;
}
void nodePermutation2nodeLocations(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &_perm_nodes,
Eigen::ArrayXi& locations)
{
locations = _perm_nodes.indices().array();
for (unsigned int i = 0; i < locations.size(); i++)
locations = (locations > locations(i)).select(locations + nodeDim(i) - 1, locations);
}
void augmentPermutation(Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm, unsigned int new_size)
{
unsigned int old_size = perm.indices().size();
unsigned int dim = new_size - old_size;
Eigen::VectorXi new_indices(new_size);
new_indices.head(old_size) = perm.indices();
new_indices.tail(dim) = Eigen::ArrayXi::LinSpaced(dim, old_size, new_size - 1);
perm.resize(new_size);
perm.indices() = new_indices;
std::cout << "permutation augmented" << std::endl;
// resize and update locations
node_locations_.conservativeResize(node_locations_.size() + 1);
node_locations_(node_locations_.size() - 1) = x_incr_.size();
std::cout << "node_locations_ augmented" << std::endl;
}
void accumulatePermutation(const Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> &perm)
{
//std::cout << std::endl << "old acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
//std::cout << "incr perm " << perm.indices().transpose() << std::endl;
// acumulate permutation
if (perm.size() == acc_node_permutation_.size()) //full permutation
acc_node_permutation_ = perm * acc_node_permutation_;
else //partial permutation
{
Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> incr_permutation_nodes(
Eigen::VectorXi::LinSpaced(nNodes(), 0, nNodes() - 1)); // identity permutation
incr_permutation_nodes.indices().tail(perm.size()) = perm.indices()
+ Eigen::VectorXi::Constant(perm.size(), nNodes() - perm.size());
//std::cout << "incr perm " << incr_permutation_nodes.indices().transpose() << std::endl;
acc_node_permutation_ = incr_permutation_nodes * acc_node_permutation_;
}
//std::cout << "new acc_permutation_nodes_ " << acc_permutation_nodes_.indices().transpose() << std::endl;
// update nodes orders and locations
nodePermutation2nodeLocations(acc_node_permutation_, node_locations_);
}
unsigned int nodeDim(const unsigned int _idx)
{
assert(_idx < nNodes());
return nodes_.at(_idx)->getSize();
}
unsigned int nodeOrder(const unsigned int _idx)
{
assert(_idx < nNodes());
assert(_idx < acc_node_permutation_.indices().size());
return acc_node_permutation_.indices()(_idx);
}
unsigned int nodeLocation(const unsigned int _idx)
{
assert(_idx < nNodes());
assert(_idx < node_locations_.size());
return node_locations_(_idx);
}
unsigned int nNodes()
{
return nodes_.size();
}
CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
{
//std::cout << "adding fac " << _fac_ptr->id() << std::endl;
//_fac_ptr->print();
switch (_fac_ptr->getTypeId())
{
case FAC_GPS_FIX_2d:
{
FactorGPS2d* specific_ptr = (FactorGPS2d*)(_fac_ptr);
return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr));
break;
}
case FAC_ODOM_2d:
{
FactorOdom2d* specific_ptr = (FactorOdom2d*)(_fac_ptr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
case FAC_CORNER_2d:
{
FactorCorner2d* specific_ptr = (FactorCorner2d*)(_fac_ptr);
return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2d, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
default:
std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()"
<< std::endl;
return nullptr;
}
}
void printResults()
{
std::cout << " solved in " << time_solving_ * 1e3 << " ms | " << R_.nonZeros() << " nonzeros in R"
<< std::endl;
std::cout << "x = " << x_incr_.transpose() << std::endl;
}
void printProblem()
{
std::cout << std::endl << "A_nodes_: " << std::endl
<< Eigen::MatrixXi::Identity(A_nodes_.rows(), A_nodes_.rows()) * A_nodes_ << std::endl << std::endl;
//std::cout << "A_: " << std::endl << MatrixXd::Identity(A_.rows(), A_.rows()) * A_ << std::endl << std::endl;
std::cout << "b_: " << std::endl << b_.transpose() << std::endl << std::endl;
}
};
} // namespace wolf
#endif /* TRUNK_SRC_SOLVER_QR_SOLVER_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)
// 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--------
/*
* solver_QR.h
*
* Created on: Jun 22, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_SOLVER_QR_H_
#define TRUNK_SRC_SOLVER_SOLVER_QR_H_
using namespace Eigen;
class SolverQR
{
protected:
SparseQR < SparseMatrix<double>, NaturalOrdering<int>> solver_;
SparseMatrix<double> A, A_ordered, R;
VectorXd b, x, x_ordered, x_ordered_partial;
int n_measurements = 0;
int n_nodes = 0;
// ordering variables
SparseMatrix<int> A_nodes_ordered;
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
CCOLAMDOrdering<int> ordering, partial_ordering;
VectorXi nodes_ordering_factors;
private:
};
#endif /* TRUNK_SRC_SOLVER_SOLVER_QR_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)
// 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 CERES_MANAGER_H_
#define CERES_MANAGER_H_
//wolf includes
#include "core/factor/factor_GPS_2d.h"
#include "core/common/wolf.h"
#include "core/state_block/state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
#include "../factor_sparse.h"
#include "../factor_odom_2d_theta.h"
#include "../factor_odom_2d_complex_angle.h"
#include "../factor_corner_2d_theta.h"
/** \brief solver manager for WOLF
*
*/
class SolverManager
{
protected:
public:
SolverManager(ceres::Problem::Options _options);
~SolverManager();
ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options);
//void computeCovariances(WolfProblemPtr _problem_ptr);
void update(const WolfProblemPtr _problem_ptr);
void addFactor(FactorBasePtr _fac_ptr);
void removeFactor(const unsigned int& _fac_idx);
void addStateUnit(StateBlockPtr _st_ptr);
void removeAllStateUnits();
void updateStateUnitStatus(StateBlockPtr _st_ptr);
ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
};
#endif
//--------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::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; });
}
void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsigned int& _n_cols)
{
A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; });
}
void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
original.coeffRef(row+ins_row, col+ins_col) += ins(ins_row,ins_col);
original.makeCompressed();
}
void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
original.insert(row+ins_row, col+ins_col) = ins(ins_row,ins_col);
original.makeCompressed();
}
}
#endif /* SPARSE_UTILS_H_ */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/* // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file factory_state_block.h #pragma once
*
* Created on: Apr 27, 2020
* \author: jsola
*/
#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
#include "core/common/factory.h" #include "core/common/factory.h"
#include "core/state_block/state_block.h" #include "core/state_block/state_block.h"
namespace wolf namespace wolf
{ {
/** \brief StateBlock factory class /** \brief StateBlock factory class
* *
* This factory can create objects of class StateBlock and classes deriving from StateBlock. * This factory can create objects of class StateBlock and classes deriving from StateBlock.
...@@ -43,18 +31,12 @@ namespace wolf ...@@ -43,18 +31,12 @@ namespace wolf
* and the TYPE of state block is identified with a string. * and the TYPE of state block is identified with a string.
* For example, the following processor types are implemented, * For example, the following processor types are implemented,
* - "StateBlock" for StateBlock * - "StateBlock" for StateBlock
* - "StatePoint3d" for StatePoint3d
* - "StatePoint2d" for StatePoint2d
* - "StateQuaternion" for StateQuaternion * - "StateQuaternion" for StateQuaternion
* - "StateAngle" for StateAngle * - "StateAngle" for StateAngle
* - "StateHomogeneous3d" for StateHomogeneous3d * - "StateHomogeneous3d" for StateHomogeneous3d
* *
* The factory also creates state blocks according to the block key used in to identify state blocks in each Wolf node.
* These keys are single-letter strings. The following letters are implemented
* - "O" for 2d orientation, creates StateAngle
* - "O" for 3d orientation, creates StateQuaternion
* - "H" crestes StateHomogeneous3d
*
* Any other letter creates the base StateBlock.
*
* Find general Factory documentation in class Factory: * Find general Factory documentation in class Factory:
* - Access the factory * - Access the factory
* - Register/unregister creators * - Register/unregister creators
...@@ -80,11 +62,7 @@ namespace wolf ...@@ -80,11 +62,7 @@ namespace wolf
* } * }
* \endcode * \endcode
* *
* #### Creating processors * #### Examples:
* Note: Prior to invoking the creation of a processor of a derived type,
* you must register the creator for this type into the factory.
*
* Note: State blocks of the base type do not need to be registered.
* *
* To create a StateQuaternion, you type: * To create a StateQuaternion, you type:
* *
...@@ -92,64 +70,41 @@ namespace wolf ...@@ -92,64 +70,41 @@ namespace wolf
* auto sq_ptr = FactoryStateBlock::create("StateQuaternion", Vector4d(1,0,0,0), false); * auto sq_ptr = FactoryStateBlock::create("StateQuaternion", Vector4d(1,0,0,0), false);
* \endcode * \endcode
* *
* If your problem has dimension 3 (e.g. is 3D), you can use the key "O" to create a StateQuaternion,
*
* \code
* auto sq_ptr = FactoryStateBlock::create("O", Vector4d(1,0,0,0), false);
* \endcode
*
* However if your problem has dimension 2 (e.g. is 2D), the key "O" will create a StateAngle,
*
* \code
* auto sa_ptr = FactoryStateBlock::create("O", Vector1d(angle_in_radians), false);
* \endcode
*
* Note: It is an error to provide state vectors of the wrong size (4 for 3D orientation, 1 for 2D).
*
* To create StateBlocks to store 2D position and velocity, you type: * To create StateBlocks to store 2D position and velocity, you type:
* *
* \code * \code
* auto sp2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false); * auto sp2_ptr = FactoryStateBlock::create("StatePoint2d", Vector2d(1,2), false);
* auto sv2_ptr = FactoryStateBlock::create("StateBlock", Vector2d(1,2), false); * auto sv2_ptr = FactoryStateBlock::create("StateVector2d", Vector2d(1,2), false);
* \endcode * \endcode
* *
* To create StateBlocks to store 2D position and velocity, you can also use the key letters:
*
* \code
* auto sp2_ptr = FactoryStateBlock::create("P", Vector2d(1,2), false);
* auto sv2_ptr = FactoryStateBlock::create("V", Vector2d(1,2), false);
* \endcode
*
* To create StateBlocks to store 3D position and velocity, you type:
*
* \code
* auto sp3_ptr = FactoryStateBlock::create("P", Vector3d(1,2,3), false);
* auto sv3_ptr = FactoryStateBlock::create("V", Vector3d(1,2,3), false);
* \endcode
*
* Note: for base state blocks, the size is determined by the size of the provided vector parameter `VectorXd& _state`.
*/ */
typedef Factory<StateBlock, const Eigen::VectorXd&, bool> FactoryStateBlock; typedef Factory<std::shared_ptr<StateBlock>, const Eigen::VectorXd&, bool> FactoryStateBlock;
template<> template <>
inline std::string FactoryStateBlock::getClass() const inline std::string FactoryStateBlock::getClass() const
{ {
return "FactoryStateBlock"; return "FactoryStateBlock";
} }
typedef Factory<std::shared_ptr<StateBlock>, bool> FactoryStateBlockIdentity;
template <>
inline std::string FactoryStateBlockIdentity::getClass() const
{
return "FactoryStateBlockIdentity";
}
typedef Factory<Eigen::VectorXd> FactoryStateBlockIdentityVector;
template <>
inline std::string FactoryStateBlockIdentityVector::getClass() const
{
return "FactoryStateBlockIdentityVector";
}
#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \ #define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
namespace \ namespace \
{ \ { \
const bool WOLF_UNUSED StateBlockType##Registered = \ const bool WOLF_UNUSED StateBlockType##Registered = \
FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \ FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); \
const bool WOLF_UNUSED StateBlockType##IdentityRegistered = \
FactoryStateBlockIdentity::registerCreator(#StateBlockType, StateBlockType::createIdentity); \
const bool WOLF_UNUSED StateBlockType##IdentityVectorRegistered = \
FactoryStateBlockIdentityVector::registerCreator(#StateBlockType, StateBlockType::Identity); \
} }
} // namespace wolf
#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
namespace \
{ \
const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
FactoryStateBlock::registerCreator(#Key, StateBlockType::create); \
}
}
#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_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)
// 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--------
/**
* \file has_state_blocks.h
*
* Created on: Aug 27, 2019
* \author: jsola
*/
#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
#include <map>
namespace wolf
{
class HasStateBlocks
{
friend Problem;
public:
HasStateBlocks();
HasStateBlocks(const StateStructure& _structure) : structure_(_structure), state_block_map_() {}
virtual ~HasStateBlocks();
const StateStructure& getStructure() const { return structure_; }
void appendToStructure(const char& _frame_type){ structure_ += _frame_type; }
bool isInStructure(const char& _sb_type) const { return structure_.find(_sb_type) != std::string::npos; }
const std::unordered_map<char, StateBlockConstPtr>& getStateBlockMap() const;
const std::unordered_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;
virtual StateBlockPtr addStateBlock(const char& _sb_type, const StateBlockPtr& _sb, ProblemPtr _problem);
virtual unsigned int removeStateBlock(const char& _sb_type);
bool hasStateBlock(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool hasStateBlock(const StateBlockConstPtr& _sb) const;
bool setStateBlock(const char _sb_type, const StateBlockPtr& _sb);
bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const;
StateBlockConstPtr getStateBlock(const char& _sb_type) const;
StateBlockPtr getStateBlock(const char& _sb_type);
std::unordered_map<char, StateBlockConstPtr>::const_iterator find(const StateBlockConstPtr& _sb) const;
std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor);
// Register/remove state blocks to/from wolf::Problem
virtual void registerNewStateBlocks(ProblemPtr _problem);
virtual void removeStateBlocks(ProblemPtr _problem);
// States
VectorComposite getState(const StateStructure& structure="") const;
void setState(const VectorComposite& _state, const bool _notify = true);
void setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const std::list<SizeEigen>& _sizes, const bool _notify = true);
void setState(const Eigen::VectorXd& _state, const StateStructure& _structure="", const bool _notify = true);
void setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify = true);
VectorXd getStateVector(const StateStructure& structure="") const;
unsigned int getSize(const StateStructure& _structure="") const;
unsigned int getLocalSize(const StateStructure& _structure="") const;
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.01);
protected:
// transform state
void transform(const VectorComposite& _transformation);
void printState(bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
private:
StateStructure structure_;
std::unordered_map<char, StateBlockPtr> state_block_map_;
std::unordered_map<char, StateBlockConstPtr> state_block_const_map_;
};
} // namespace wolf
//////////// IMPLEMENTATION /////////////
#include "core/state_block/state_block.h"
namespace wolf{
inline HasStateBlocks::HasStateBlocks() :
structure_(std::string("")),
state_block_map_(),
state_block_const_map_()
{
//
}
inline HasStateBlocks::~HasStateBlocks()
{
//
}
inline const std::unordered_map<char, StateBlockConstPtr>& HasStateBlocks::getStateBlockMap() const
{
return state_block_const_map_;
}
inline const std::unordered_map<char, StateBlockPtr>& HasStateBlocks::getStateBlockMap()
{
return state_block_map_;
}
inline std::vector<StateBlockConstPtr> HasStateBlocks::getStateBlockVec() const
{
std::vector<StateBlockConstPtr> sbv;
for (auto& key : structure_)
{
sbv.push_back(getStateBlock(key));
}
return sbv;
}
inline std::vector<StateBlockPtr> HasStateBlocks::getStateBlockVec()
{
std::vector<StateBlockPtr> sbv;
for (auto& key : structure_)
{
sbv.push_back(getStateBlock(key));
}
return sbv;
}
inline unsigned int HasStateBlocks::removeStateBlock(const char& _sb_type)
{
return state_block_map_.erase(_sb_type);
return state_block_const_map_.erase(_sb_type);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> HasStateBlocks::emplaceStateBlock(const char& _sb_type, ProblemPtr _problem, Args&&... _args_of_derived_state_block_constructor)
{
assert(state_block_map_.count(_sb_type) == 0 && state_block_const_map_.count(_sb_type) == 0 && "Trying to add a state block with an existing type! Use setStateBlock instead.");
std::shared_ptr<SB> sb = std::make_shared<SB>(std::forward<Args>(_args_of_derived_state_block_constructor)...);
addStateBlock(_sb_type, sb, _problem);
return sb;
}
inline bool HasStateBlocks::setStateBlock(const char _sb_type, const StateBlockPtr& _sb)
{
assert (structure_.find(_sb_type) > 0 && "Cannot set a state block out of the state structure! Use addStateBlock instead.");
assert ( state_block_map_.count(_sb_type) > 0 && state_block_const_map_.count(_sb_type) > 0 && "Cannot set an inexistent state block! Use addStateBlock instead.");
state_block_map_.at(_sb_type) = _sb;
state_block_const_map_.at(_sb_type) = _sb;
return true; // success
}
inline wolf::StateBlockConstPtr HasStateBlocks::getStateBlock(const char& _sb_type) const
{
auto it = state_block_const_map_.find(_sb_type);
if (it != state_block_const_map_.end())
return it->second;
else
return nullptr;
}
inline wolf::StateBlockPtr HasStateBlocks::getStateBlock(const char& _sb_type)
{
auto it = state_block_map_.find(_sb_type);
if (it != state_block_map_.end())
return it->second;
else
return nullptr;
}
inline wolf::StateBlockConstPtr HasStateBlocks::getP() const
{
return getStateBlock('P');
}
inline wolf::StateBlockPtr HasStateBlocks::getP()
{
return getStateBlock('P');
}
inline wolf::StateBlockConstPtr HasStateBlocks::getO() const
{
return getStateBlock('O');
}
inline wolf::StateBlockPtr HasStateBlocks::getO()
{
return getStateBlock('O');
}
inline void HasStateBlocks::fix()
{
for (auto pair_key_sbp : state_block_map_)
if (pair_key_sbp.second != nullptr)
pair_key_sbp.second->fix();
}
inline void HasStateBlocks::unfix()
{
for (auto pair_key_sbp : state_block_map_)
if (pair_key_sbp.second != nullptr)
pair_key_sbp.second->unfix();
}
inline bool HasStateBlocks::isFixed() const
{
bool fixed = true;
for (auto pair_key_sbp : state_block_map_)
{
if (pair_key_sbp.second)
fixed &= pair_key_sbp.second->isFixed();
}
return fixed;
}
inline void HasStateBlocks::setState(const VectorComposite& _state, const bool _notify)
{
for (const auto& pair_key_vec : _state)
{
const auto& key = pair_key_vec.first;
const auto& vec = pair_key_vec.second;
const auto& sb = getStateBlock(key);
if (!sb)
WOLF_ERROR("Stateblock key ", key, " not in the structure");
assert (sb && "Stateblock key not in the structure");
sb->setState(vec, _notify);
}
}
inline void HasStateBlocks::setState(const Eigen::VectorXd& _state, const StateStructure& _structure, const bool _notify)
{
StateStructure structure;
if (_structure == "")
structure = structure_;
else
structure = _structure;
int size = getSize(structure);
assert(_state.size() == size && "In FrameBase::setState wrong state size");
unsigned int index = 0;
for (const char key : structure)
{
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
sb->setState(_state.segment(index, sb->getSize()), _notify); // do not notify if state block is not estimated by the solver
index += sb->getSize();
}
}
inline void HasStateBlocks::setState (const Eigen::VectorXd& _state,
const StateStructure& _structure,
const std::list<SizeEigen>& _sizes,
const bool _notify)
{
SizeEigen index = 0;
auto size_it = _sizes.begin();
for (const auto& key : _structure)
{
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
assert(*size_it == sb->getSize() && "State block size mismatch");
sb->setState(_state.segment(index, *size_it), _notify);
index += *size_it;
size_it ++;
}
}
inline void HasStateBlocks::setState(const StateStructure& _structure, const std::list<VectorXd>& _vectors, const bool _notify)
{
auto vec_it = _vectors.begin();
for (const auto key : _structure)
{
const auto& sb = getStateBlock(key);
assert (sb && "Stateblock key not in the structure");
assert(vec_it->size() == sb->getSize() && "State block size mismatch");
sb->setState(*vec_it, _notify);
vec_it ++;
}
}
//// _structure can be either stateblock structure of the node or a subset of this structure
inline VectorXd HasStateBlocks::getStateVector(const StateStructure& _structure) const
{
StateStructure structure;
if (_structure == "")
structure = structure_;
else
structure = _structure;
VectorXd state(getSize(structure));
unsigned int index = 0;
for (const char key : structure)
{
const auto& sb = getStateBlock(key);
assert(sb != nullptr && "Requested StateBlock key not in the structure");
state.segment(index,sb->getSize()) = sb->getState();
index += sb->getSize();
}
return state;
}
inline VectorComposite HasStateBlocks::getState(const StateStructure& _structure) const
{
const StateStructure& structure = (_structure == "" ? structure_ : _structure);
VectorComposite state;
for (const auto key : structure)
{
auto state_it = state_block_map_.find(key);
if (state_it != state_block_map_.end())
state.emplace(key, state_it->second->getState());
}
return state;
}
inline unsigned int HasStateBlocks::getSize(const StateStructure& _structure) const
{
const StateStructure& structure = (_structure == "" ? structure_ : _structure);
unsigned int size = 0;
for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
WOLF_ERROR("Stateblock key ", key, " not in the structure");
}
size += sb->getSize();
}
return size;
}
inline std::unordered_map<char, StateBlockConstPtr>::const_iterator HasStateBlocks::find(const StateBlockConstPtr& _sb) const
{
const auto& it = std::find_if(state_block_const_map_.begin(),
state_block_const_map_.end(),
[_sb](const std::pair<char, StateBlockConstPtr>& pair)
{
return pair.second == _sb;
}
);
return it;
}
inline std::unordered_map<char, StateBlockPtr>::const_iterator HasStateBlocks::find(const StateBlockPtr& _sb)
{
const auto& it = std::find_if(state_block_map_.begin(),
state_block_map_.end(),
[_sb](const std::pair<char, StateBlockPtr>& pair)
{
return pair.second == _sb;
}
);
return it;
}
inline bool HasStateBlocks::hasStateBlock(const StateBlockConstPtr &_sb) const
{
return this->find(_sb) != state_block_const_map_.end();
}
inline bool HasStateBlocks::stateBlockKey(const StateBlockConstPtr &_sb, char& _key) const
{
const auto& it = this->find(_sb);
bool found = (it != state_block_const_map_.end());
if (found)
{
_key = it->first;
return true;
}
else
{
_key = '0'; // '0' = not found
return false;
}
}
inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structure) const
{
StateStructure structure;
if (_structure == "")
structure = structure_;
else
structure = _structure;
unsigned int size = 0;
for (const char key : structure)
{
const auto& sb = getStateBlock(key);
if (!sb){
WOLF_ERROR("Stateblock key ", key, " not in the structure");
}
size += sb->getLocalSize();
}
return size;
}
} // namespace wolf
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
//--------LICENSE_START-------- // WOLF - Copyright (C) 2020-2025
// // Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// 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) and
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) // Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved. // 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 // 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 // it under the terms of the GNU General Public License version 3
// the Free Software Foundation, either version 3 of the License, or // as published by the Free Software Foundation.
// at your option) any later version.
// //
// This program is distributed in the hope that it will be useful, // This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of // but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details. // GNU 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-------- // You should have received a copy of the GNU General Public License
/** // along with this program. If not, see <http://www.gnu.org/licenses/>.
* \file local_parametrization_angle.h #pragma once
*
* Created on: Apr 4, 2017
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
#define LOCAL_PARAMETRIZATION_ANGLE_H_
#include "core/state_block/local_parametrization_base.h" #include "core/state_block/local_parametrization_base.h"
#include "core/math/rotations.h" #include "core/math/rotations.h"
namespace wolf namespace wolf
{ {
class LocalParametrizationAngle : public LocalParametrizationBase class LocalParametrizationAngle : public LocalParametrizationBase
{ {
public: public:
LocalParametrizationAngle(); LocalParametrizationAngle();
~LocalParametrizationAngle() override; ~LocalParametrizationAngle() override;
bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta, bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override; Eigen::Map<const Eigen::VectorXd>& _delta,
bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override; bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
bool minus(Eigen::Map<const Eigen::VectorXd>& _x1, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
Eigen::Map<const Eigen::VectorXd>& _x2, bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override; Eigen::Map<const Eigen::VectorXd>& _x2,
bool isValid(const Eigen::VectorXd& state, double tolerance) override; Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) override;
bool isValid(const Eigen::VectorXd& state, double tolerance) override;
}; };
inline LocalParametrizationAngle::LocalParametrizationAngle() : inline LocalParametrizationAngle::LocalParametrizationAngle() : LocalParametrizationBase(1, 1)
LocalParametrizationBase(1,1)
{ {
// //
} }
...@@ -64,14 +52,14 @@ inline LocalParametrizationAngle::~LocalParametrizationAngle() ...@@ -64,14 +52,14 @@ inline LocalParametrizationAngle::~LocalParametrizationAngle()
inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h, inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta, Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{ {
_h_plus_delta(0) = pi2pi(_h(0) + _delta(0)); _h_plus_delta(0) = pi2pi(_h(0) + _delta(0));
return true; return true;
} }
inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const
{ {
_jacobian(0) = 1.0; _jacobian(0) = 1.0;
return true; return true;
...@@ -79,19 +67,17 @@ inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::V ...@@ -79,19 +67,17 @@ inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::V
inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1, inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2, Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) Eigen::Map<Eigen::VectorXd>& _x2_minus_x1)
{ {
_x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0)); _x2_minus_x1(0) = pi2pi(_x2(0) - _x1(0));
return true; return true;
} }
inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance) inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, double tolerance)
{ {
//Any real is a valid angle because we use the pi2pi function. Also // Any real is a valid angle because we use the pi2pi function. Also
//the types don't match. In this case the argument is // the types don't match. In this case the argument is
//Eigen::Map not Eigen::VectorXd // Eigen::Map not Eigen::VectorXd
return true; return true;
} }
} /* namespace wolf */ } /* namespace wolf */
#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */