Skip to content
Snippets Groups Projects

Compare revisions

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

Source

Select target project
No results found

Target

Select target project
  • mobile_robotics/wolf_projects/wolf_lib/wolf
1 result
Show changes
View open merge request
Hide whitespace changes
Inline Side-by-side
Showing
with 999 additions and 1804 deletions
include/core/state_block/local_parametrization_base.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_base.h
*
* Created on: Feb 17, 2016
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_BASE_H_
#define LOCAL_PARAMETRIZATION_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
namespace wolf {
namespace wolf
{
class LocalParametrizationBase
{
protected:
unsigned int global_size_;
unsigned int local_size_;
class LocalParametrizationBase{
protected:
unsigned int global_size_;
unsigned int local_size_;
public:
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
public:
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
bool plus(const Eigen::VectorXd& _x,
const Eigen::VectorXd& _delta,
Eigen::VectorXd& _x_plus_delta) const;
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
bool plus(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, Eigen::VectorXd& _x_plus_delta) const;
virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const = 0;
virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
Eigen::Map<const Eigen::VectorXd>& _x2,
Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
virtual bool isValid(const Eigen::VectorXd& state, double tolerance) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
};
} // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_BASE_H_ */
} // namespace wolf
include/core/state_block/local_parametrization_homogeneous.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_homogeneous.h
*
* Created on: 24/02/2016
* Author: jsola
*/
#ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
#define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/local_parametrization_base.h"
namespace wolf {
namespace wolf
{
/**
* \brief Local parametrization for homogeneous vectors.
*
......@@ -47,7 +36,8 @@ namespace wolf {
* where \f$\otimes\f$ is the quaternion product, and \f$d{\bf q} = {\bf q}(d {\bf x})\f$ is a unit delta_quaternion
* equivalent to a rotation \f$d{\bf x}\f$, obtained with
*
* \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|) \end{array}\right]\f]
* \f[{\bf q}(d{\bf x}) = \left[\begin{array}{c} \frac{ d {\bf x}}{ |d{\bf x}|} \sin(|d{\bf x}|) \\ \cos(|d{\bf x}|)
* \end{array}\right]\f]
*
* Contrary to the case of quaternions, it is not required that \f${\bf x}\f$ be a unit homogeneous vector.
* In this case, the updated \f${\bf x}^+={\bf x}\oplus d {\bf x}\f$ has the same norm as \f${\bf x}\f$.
......@@ -57,20 +47,19 @@ namespace wolf {
*/
class LocalParametrizationHomogeneous : public LocalParametrizationBase
{
public:
LocalParametrizationHomogeneous();
~LocalParametrizationHomogeneous() override;
public:
LocalParametrizationHomogeneous();
~LocalParametrizationHomogeneous() override;
bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
bool isValid(const Eigen::VectorXd& state, double tolerance) override;
bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<const Eigen::VectorXd>& _delta,
Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const override;
bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
Eigen::Map<const Eigen::VectorXd>& _h2,
Eigen::Map<Eigen::VectorXd>& _h2_minus_h1) override;
bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
} // namespace wolf
#endif /* LOCALPARAMETRIZATIONHOMOGENEOUS_H_ */
} // namespace wolf
include/core/state_block/local_parametrization_quaternion.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_quaternion.h
*
* Created on: Feb 18, 2016
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
#define LOCAL_PARAMETRIZATION_QUATERNION_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/local_parametrization_base.h"
namespace wolf {
namespace wolf
{
/**
* \brief Local or global orientation error
*
* Local or global orientation error.
*
* This enum controls whether the orientation error is composed locally or globally to an orientation specification.
*
* See LocalParametrizationQuaternion for more information.
*/
typedef enum {
* \brief Local or global orientation error
*
* Local or global orientation error.
*
* This enum controls whether the orientation error is composed locally or globally to an orientation specification.
*
* See LocalParametrizationQuaternion for more information.
*/
typedef enum
{
DQ_LOCAL,
DQ_GLOBAL
} QuaternionDeltaReference;
......@@ -71,40 +61,33 @@ typedef enum {
template <QuaternionDeltaReference DeltaReference = DQ_LOCAL>
class LocalParametrizationQuaternion : public LocalParametrizationBase
{
public:
LocalParametrizationQuaternion() : LocalParametrizationBase(4, 3)
{
//
}
public:
~LocalParametrizationQuaternion() override
{
//
}
LocalParametrizationQuaternion() :
LocalParametrizationBase(4, 3)
{
//
}
bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override;
~LocalParametrizationQuaternion() override
{
//
}
bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<const Eigen::VectorXd>& _delta_theta,
Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const override;
bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
// inline bool isValid(const Eigen::VectorXd& state) override;
// template<QuaternionDeltaReference DeltaReference>
bool isValid(const Eigen::VectorXd& _state, double tolerance) override
{
return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
}
bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
Eigen::Map<Eigen::MatrixRowXd>& _jacobian) const override;
bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
Eigen::Map<const Eigen::VectorXd>& _q2,
Eigen::Map<Eigen::VectorXd>& _q2_minus_q1) override;
bool isValid(const Eigen::VectorXd& _state, double tolerance) override
{
return _state.size() == global_size_ && fabs(1.0 - _state.norm()) < tolerance;
}
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal;
} // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */
} // namespace wolf
include/core/state_block/state_angle.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/**
* \file state_angle.h
*
* Created on: Apr 4, 2017
* \author: jsola
*/
#ifndef STATE_ANGLE_H_
#define STATE_ANGLE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_angle.h"
namespace wolf
{
class StateAngle : public StateBlock
{
public:
StateAngle(double _angle = 0.0, bool _fixed = false, bool _transformable = true);
StateAngle(const Vector1d& _angle, bool _fixed = false, bool _transformable = true);
public:
StateAngle(double _angle, bool _fixed = false, bool _transformable = true);
StateAngle(const Eigen::VectorXd& _angle, bool _fixed = false, bool _transformable = true);
static Eigen::VectorXd Identity();
~StateAngle() override;
WOLF_STATE_BLOCK_CREATE(StateAngle);
virtual void transform(const VectorComposite& _transformation) override;
~StateAngle() override;
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
virtual void transform(const VectorComposite& _transformation) override;
};
inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable) :
StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
inline StateAngle::StateAngle(double _angle, bool _fixed, bool _transformable)
: StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
state_(0) = pi2pi(_angle);
}
inline StateAngle::StateAngle(const Vector1d& _angle, bool _fixed, bool _transformable) :
StateBlock(1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
inline StateAngle::StateAngle(const Eigen::VectorXd& _angle, bool _fixed, bool _transformable)
: StateBlock("StateAngle", 1, _fixed, std::make_shared<LocalParametrizationAngle>(), _transformable)
{
if (_angle.size() != 1) throw std::runtime_error("The angle state vector must be of size 1!");
state_(0) = pi2pi(_angle(0));
}
inline StateAngle::~StateAngle()
inline Eigen::VectorXd StateAngle::Identity()
{
//
return Eigen::VectorXd::Zero(1);
}
inline StateBlockPtr StateAngle::create (const Eigen::VectorXd& _state, bool _fixed)
inline StateAngle::~StateAngle()
{
MatrixSizeCheck<1,1>::check(_state);
return std::make_shared<StateAngle>(_state(0), _fixed);
//
}
inline void StateAngle::transform(const VectorComposite& _transformation)
{
if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
}
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
include/core/state_block/state_block.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#ifndef STATE_BLOCK_H_
#define STATE_BLOCK_H_
/*
* Macro for defining StateBlocks creators.
*
* Place a call to this macro inside your class declaration
* (in the state_block_class.h file), preferably just after the constructors.
*
* In order to use this macro, the derived state_block class, StateBlockClass,
* must have the constructor callable as:
*
* StateBlockClass(const Eigen::VectorXd& _state, bool _fixed)
*
* And the static method:
*
* static Eigen::VectorXd Identity()
*/
#define WOLF_STATE_BLOCK_CREATE(StateBlockClass) \
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed) \
{ \
return std::make_shared<StateBlockClass>(_state, _fixed); \
} \
static StateBlockPtr createIdentity(bool _fixed) \
{ \
return std::make_shared<StateBlockClass>(StateBlockClass::Identity(), _fixed); \
}
// Fwd references
namespace wolf{
namespace wolf
{
class NodeBase;
class LocalParametrizationBase;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
#include "core/composite/vector_composite.h"
//std includes
// std includes
#include <iostream>
#include <mutex>
namespace wolf {
namespace wolf
{
/** \brief class StateBlock
*
* This class implements a state block for general nonlinear optimization. It offers the following functionality:
......@@ -50,193 +71,226 @@ namespace wolf {
*/
class StateBlock : public std::enable_shared_from_this<StateBlock>
{
public:
protected:
std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
std::atomic<SizeEigen> state_size_; ///< State vector size
Eigen::VectorXd state_; ///< State vector storing the state values
mutable std::mutex mut_state_; ///< State vector mutex
LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
std::atomic_bool local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
public:
/** \brief Constructor from size
*
* \param _size is state size
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
*/
StateBlock(const SizeEigen _size, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
/** \brief Constructor from vector
*
* \param _state is state vector
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr, bool _transformable = true);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
StateBlock(StateBlock&& o) = delete;
StateBlock& operator=(const StateBlock& o) = delete;
/** \brief Destructor
**/
virtual ~StateBlock();
/** \brief Returns the state vector
**/
Eigen::VectorXd getState() const;
/** \brief Returns the state vector data array
**/
double* getStateData();
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
/** \brief Returns the state size
**/
SizeEigen getSize() const;
public:
protected:
std::string type_; ///< Type of the derived class
std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
std::atomic<SizeEigen> state_size_; ///< State vector size
Eigen::VectorXd state_; ///< State vector storing the state values
mutable std::mutex mut_state_; ///< State vector mutex
LocalParametrizationBasePtr
local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
std::atomic_bool state_updated_; ///< Flag to indicate whether the state has been updated or not
std::atomic_bool fix_updated_; ///< Flag to indicate whether the status has been updated or not
std::atomic_bool
local_param_updated_; ///< Flag to indicate whether the local_parameterization has been updated or not
bool transformable_; ///< Flag to indicate if the state block can be transformed to another reference frame
public:
/** \brief Constructor from size
*
* \param _size is state size
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
*/
StateBlock(const std::string& _type,
const SizeEigen _size,
bool _fixed = false,
LocalParametrizationBasePtr _local_param_ptr = nullptr,
bool _transformable = true);
/** \brief Constructor from vector
*
* \param _state is state vector
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
StateBlock(const std::string& _type,
const Eigen::VectorXd& _state,
bool _fixed = false,
LocalParametrizationBasePtr _local_param_ptr = nullptr,
bool _transformable = true);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
StateBlock(StateBlock&& o) = delete;
StateBlock& operator=(const StateBlock& o) = delete;
/** \brief Destructor
**/
virtual ~StateBlock();
/** \brief Returns the type of the state block
**/
std::string getType() const;
/** \brief Returns the state vector
**/
Eigen::VectorXd getState() const;
/** \brief Returns the state vector data array
**/
double* getStateData();
/** \brief Sets the state vector
**/
void setState(const Eigen::VectorXd& _state, const bool _notify = true);
/** \brief Returns the state size
**/
SizeEigen getSize() const;
/**\brief Returns the size of the local parametrization
*/
SizeEigen getLocalSize() const;
/** \brief Returns if the state is fixed (not estimated)
**/
bool isFixed() const;
/** \brief Sets the state as fixed
**/
void fix();
/** \brief Sets the state as estimated
**/
void unfix();
/** \brief Sets the state status
**/
void setFixed(bool _fixed);
/** \brief Returns if the state has a local parametrization
**/
bool hasLocalParametrization() const;
/** \brief Returns the state local parametrization ptr
**/
LocalParametrizationBasePtr getLocalParametrization() const;
/** \brief Sets a local parametrization
**/
void setLocalParametrization(LocalParametrizationBasePtr _local_param);
/** \brief Removes the state_block local parametrization
**/
void removeLocalParametrization();
/** \brief Return if state has been updated
**/
bool stateUpdated() const;
/** \brief Return if fix/unfix has been updated
**/
bool fixUpdated() const;
/** \brief Return if local parameterization has been updated
**/
bool localParamUpdated() const;
/** \brief Set state_updated_ to false
**/
void resetStateUpdated();
/** \brief Set fix_updated_ to false
**/
void resetFixUpdated();
/** \brief Set localk_param_updated_ to false
**/
void resetLocalParamUpdated();
virtual void setIdentity(bool _notify = true);
void setZero(bool _notify = true);
virtual Eigen::VectorXd identity() const;
Eigen::VectorXd zero() const;
/**\brief Returns the size of the local parametrization
*/
SizeEigen getLocalSize() const;
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.1);
/** \brief Returns if the state is fixed (not estimated)
**/
bool isFixed() const;
bool isTransformable() const
{
return transformable_;
}
/** \brief Sets the state as fixed
**/
void fix();
void setTransformable(bool _trf = true)
{
transformable_ = _trf;
}
/** \brief Sets the state as estimated
**/
void unfix();
void setNonTransformable()
{
transformable_ = false;
}
/** \brief Sets the state status
**/
void setFixed(bool _fixed);
virtual void transform(const VectorComposite& _transformation) = 0;
/** \brief Returns if the state has a local parametrization
**/
bool hasLocalParametrization() const;
/** \brief Returns the state local parametrization ptr
**/
LocalParametrizationBasePtr getLocalParametrization() const;
/** \brief Sets a local parametrization
**/
void setLocalParametrization(LocalParametrizationBasePtr _local_param);
/** \brief Removes the state_block local parametrization
**/
void removeLocalParametrization();
/** \brief Return if state has been updated
**/
bool stateUpdated() const;
/** \brief Return if fix/unfix has been updated
**/
bool fixUpdated() const;
/** \brief Return if local parameterization has been updated
**/
bool localParamUpdated() const;
/** \brief Set state_updated_ to false
**/
void resetStateUpdated();
/** \brief Set fix_updated_ to false
**/
void resetFixUpdated();
/** \brief Set localk_param_updated_ to false
**/
void resetLocalParamUpdated();
virtual void setIdentity(bool _notify = true);
void setZero (bool _notify = true);
virtual Eigen::VectorXd identity() const;
Eigen::VectorXd zero() const;
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.1);
bool isTransformable() const {
return transformable_;
}
void setTransformable(bool _trf = true) {transformable_ = _trf;}
void setNonTransformable() {transformable_ = false;}
virtual void transform(const VectorComposite& _transformation) = 0;
void plus(const Eigen::VectorXd& _dv);
bool isValid(double tolerance = Constants::EPS);
void plus(const Eigen::VectorXd& _dv);
bool isValid(double tolerance = Constants::EPS);
};
} // namespace wolf
} // namespace wolf
// IMPLEMENTATION
#include "core/state_block/local_parametrization_base.h"
#include "core/common/node_base.h"
#include "core/problem/problem.h"
namespace wolf {
inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_state.size()),
state_(_state),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
local_param_updated_(false),
transformable_(_transformable)
namespace wolf
{
// std::cout << "constructed +sb" << std::endl;
inline StateBlock::StateBlock(const std::string& _type,
const Eigen::VectorXd& _state,
bool _fixed,
LocalParametrizationBasePtr _local_param_ptr,
bool _transformable)
: type_(_type),
fixed_(_fixed),
state_size_(_state.size()),
state_(_state),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
local_param_updated_(false),
transformable_(_transformable)
{
// std::cout << "constructed +sb" << std::endl;
}
inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametrizationBasePtr _local_param_ptr, bool _transformable) :
fixed_(_fixed),
state_size_(_size),
state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
local_param_updated_(false),
transformable_(_transformable)
inline StateBlock::StateBlock(const std::string& _type,
const SizeEigen _size,
bool _fixed,
LocalParametrizationBasePtr _local_param_ptr,
bool _transformable)
: type_(_type),
fixed_(_fixed),
state_size_(_size),
state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
local_param_updated_(false),
transformable_(_transformable)
{
// std::cout << "constructed +sb" << std::endl;
}
inline StateBlock::~StateBlock()
{
// std::cout << "destructed -sb" << std::endl;
// std::cout << "destructed -sb" << std::endl;
}
inline std::string StateBlock::getType() const
{
return type_;
}
inline Eigen::VectorXd StateBlock::getState() const
......@@ -252,8 +306,7 @@ inline SizeEigen StateBlock::getSize() const
inline SizeEigen StateBlock::getLocalSize() const
{
if(local_param_ptr_)
return local_param_ptr_->getLocalSize();
if (local_param_ptr_) return local_param_ptr_->getLocalSize();
return getSize();
}
......@@ -284,14 +337,14 @@ inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
inline void StateBlock::removeLocalParametrization()
{
assert(local_param_ptr_ != nullptr && "state block without local parametrization");
assert(local_param_ptr_ != nullptr && "state block without local parametrization");
local_param_ptr_.reset();
local_param_updated_.store(true);
}
inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
{
assert(_local_param != nullptr && "setting a null local parametrization");
assert(_local_param != nullptr && "setting a null local parametrization");
local_param_ptr_ = _local_param;
local_param_updated_.store(true);
}
......@@ -333,7 +386,7 @@ inline double* StateBlock::getStateData()
inline void StateBlock::setIdentity(bool _notify)
{
setState( Eigen::VectorXd::Zero(state_size_), _notify );
setState(Eigen::VectorXd::Zero(state_size_), _notify);
}
inline void StateBlock::setZero(bool _notify)
......@@ -345,7 +398,8 @@ inline Eigen::VectorXd StateBlock::identity() const
{
return Eigen::VectorXd::Zero(state_size_);
}
inline Eigen::VectorXd StateBlock::zero() const
inline Eigen::VectorXd StateBlock::zero() const
{
return identity();
}
......@@ -354,6 +408,5 @@ inline bool StateBlock::isValid(double tolerance)
{
return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
}
}// namespace wolf
#endif
} // namespace wolf
include/core/state_block/state_block_derived.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef STATE_BLOCK_DERIVED_H_
#define STATE_BLOCK_DERIVED_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/state_block.h"
namespace wolf
{
/**
* @brief State block for general parameters
*
......@@ -40,12 +35,22 @@ template <size_t size>
class StateParams : public StateBlock
{
public:
StateParams(const Eigen::VectorXd& _state, bool _fixed = false) : StateBlock(_state, _fixed, nullptr, false) {}
StateParams(const Eigen::VectorXd& _state, bool _fixed = false)
: StateBlock("StateParams" + std::to_string(size), _state, _fixed, nullptr, false)
{
if (_state.size() != size)
throw std::runtime_error("Wrong vector size for StateParams" + std::to_string(size) + ":" +
std::to_string(_state.size()));
}
static Eigen::VectorXd Identity()
{
return Eigen::Matrix<double, size, 1>::Zero();
}
WOLF_STATE_BLOCK_CREATE(StateParams<size>);
void transform(const VectorComposite& _transformation) override
{
// non transformable! --> do nothing
}
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
typedef StateParams<1> StateParams1;
......@@ -58,6 +63,16 @@ typedef StateParams<7> StateParams7;
typedef StateParams<8> StateParams8;
typedef StateParams<9> StateParams9;
typedef StateParams<10> StateParams10;
typedef StateParams<11> StateParams11;
typedef StateParams<12> StateParams12;
typedef StateParams<13> StateParams13;
typedef StateParams<14> StateParams14;
typedef StateParams<15> StateParams15;
typedef StateParams<16> StateParams16;
typedef StateParams<17> StateParams17;
typedef StateParams<18> StateParams18;
typedef StateParams<19> StateParams19;
typedef StateParams<20> StateParams20;
/**
* @brief State block for general 2D points and positions
......@@ -72,14 +87,21 @@ class StatePoint2d : public StateBlock
{
public:
StatePoint2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock(_state, _fixed, nullptr, _transformable)
: StateBlock("StatePoint2d", _state, _fixed, nullptr, _transformable)
{
if (_state.size() != 2)
throw std::runtime_error("Wrong vector size for StatePoint2d: " + std::to_string(_state.size()));
}
static Eigen::VectorXd Identity()
{
return Eigen::Vector2d::Zero();
}
WOLF_STATE_BLOCK_CREATE(StatePoint2d);
void transform(const VectorComposite& _transformation) override
{
if (transformable_) setState(_transformation.at('P') + Rotation2Dd(_transformation.at('O')(0)) * getState());
if (transformable_)
setState(_transformation.at('P') + Eigen::Rotation2Dd(_transformation.at('O')(0)) * getState());
}
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
......@@ -96,14 +118,20 @@ class StateVector2d : public StateBlock
{
public:
StateVector2d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock(_state, _fixed, nullptr, _transformable)
: StateBlock("StateVector2d", _state, _fixed, nullptr, _transformable)
{
if (_state.size() != 2)
throw std::runtime_error("Wrong vector size for StateVector2d: " + std::to_string(_state.size()));
}
static Eigen::VectorXd Identity()
{
return Eigen::Vector2d::Zero();
}
WOLF_STATE_BLOCK_CREATE(StateVector2d);
void transform(const VectorComposite& _transformation) override
{
if (transformable_) setState(Rotation2Dd(_transformation.at('O')(0)) * getState());
if (transformable_) setState(Eigen::Rotation2Dd(_transformation.at('O')(0)) * getState());
}
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
......@@ -119,15 +147,21 @@ class StatePoint3d : public StateBlock
{
public:
StatePoint3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock(_state, _fixed, nullptr, _transformable)
: StateBlock("StatePoint3d", _state, _fixed, nullptr, _transformable)
{
if (_state.size() != 3)
throw std::runtime_error("Wrong vector size for StatePoint3d: " + std::to_string(_state.size()));
}
static Eigen::VectorXd Identity()
{
return Eigen::Vector3d::Zero();
}
WOLF_STATE_BLOCK_CREATE(StatePoint3d);
void transform(const VectorComposite& _transformation) override
{
if (transformable_)
setState(_transformation.at('P') + Quaterniond(_transformation.at('O').data()) * getState());
setState(_transformation.at('P') + Eigen::Quaterniond(_transformation.at('O').data()) * getState());
}
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
/**
......@@ -144,17 +178,20 @@ class StateVector3d : public StateBlock
{
public:
StateVector3d(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true)
: StateBlock(_state, _fixed, nullptr, _transformable)
: StateBlock("StateVector3d", _state, _fixed, nullptr, _transformable)
{
if (_state.size() != 3)
throw std::runtime_error("Wrong vector size for StateVector3d: " + std::to_string(_state.size()));
}
static Eigen::VectorXd Identity()
{
return Eigen::Vector3d::Zero();
}
WOLF_STATE_BLOCK_CREATE(StateVector3d);
void transform(const VectorComposite& _transformation) override
{
if (transformable_) setState(Quaterniond(_transformation.at('O').data()) * getState());
if (transformable_) setState(Eigen::Quaterniond(_transformation.at('O').data()) * getState());
}
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
};
} // namespace wolf
#endif // STATE_BLOCK_DERIVED_H_
include/core/state_block/state_composite.h deleted 100644 → 0
View file @ c7bb5751
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* state_composite.h
*
* Created on: Apr 6, 2020
* Author: jsola
*/
#ifndef STATE_BLOCK_STATE_COMPOSITE_H_
#define STATE_BLOCK_STATE_COMPOSITE_H_
#include "core/common/wolf.h"
#include <unordered_map>
#include <iostream>
namespace wolf
{
using std::string;
using namespace Eigen;
typedef std::string StateStructure;
typedef std::unordered_map < char, StateBlockPtr > StateBlockMap;
typedef StateBlockMap::const_iterator StateBlockMapCIter;
class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
{
public:
VectorComposite() {};
VectorComposite(const StateStructure& _s);
VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
/**
* \brief Construct from Eigen::VectorXd and structure
*
* Usage:
*
* VectorXd vec_eigen(1,2,3,4,5);
*
* VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
*
* result:
*
* vec_comp["a"].transpose(); // = (1,2);
* vec_comp["b"].transpose(); // = (3,4,5);
*/
VectorComposite(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
VectorComposite(const StateStructure& _structure, const std::list<VectorXd>& _vectors);
Eigen::VectorXd vector(const StateStructure& _structure) const;
/**
* \brief set from Eigen::VectorXd and structure
*
* Usage:
*
* Eigen::VectorXd vec_eigen;
* wolf::VectorComposite vec_comp;
*
* vec_comp.set( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
*
* result:
*
* vec_comp["a"].transpose(); // = (1,2);
* vec_comp["b"].transpose(); // = (3,4,5);
*/
void set(const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes);
void setZero();
bool includesStructure(const StateStructure &_structure) const;
friend std::ostream& operator <<(std::ostream &_os, const wolf::VectorComposite &_x);
friend wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y);
friend wolf::VectorComposite operator -(const wolf::VectorComposite &_x);
};
class MatrixComposite : public std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >
{
private:
std::unordered_map < char, unsigned int> size_rows_, size_cols_;
unsigned int rows() const;
unsigned int cols() const;
void sizeAndIndices(const StateStructure & _struct_rows,
const StateStructure & _struct_cols,
std::unordered_map < char, unsigned int>& _indices_rows,
std::unordered_map < char, unsigned int>& _indices_cols,
unsigned int& _rows,
unsigned int& _cols) const;
public:
MatrixComposite() {};
MatrixComposite(const StateStructure& _row_structure,
const StateStructure& _col_structure);
MatrixComposite(const StateStructure& _row_structure,
const std::list<int>& _row_sizes,
const StateStructure& _col_structure,
const std::list<int>& _col_sizes);
/**
* \brief Construct from Eigen::VectorXd and structure
*
* Usage:
*
* VectorXd vec_eigen(1,2,3,4,5);
*
* VectorComposite vec_comp( vec_eigen, "ab", {2,3} ); // vec_eigen must be at least size 5 !!
*
* result:
*
* vec_comp["a"].transpose(); // = (1,2);
* vec_comp["b"].transpose(); // = (3,4,5);
*/
MatrixComposite(const MatrixXd& _m,
const StateStructure& _row_structure,
const std::list<int>& _row_sizes,
const StateStructure& _col_structure,
const std::list<int>& _col_sizes);
~MatrixComposite() = default;
bool check() const;
static MatrixComposite zero(const StateStructure& _row_structure,
const std::list<int>& _row_sizes,
const StateStructure& _col_structure,
const std::list<int>& _col_sizes);
static MatrixComposite identity(const StateStructure& _structure,
const std::list<int>& _sizes);
unsigned int count(const char &_row,
const char &_col) const;
bool emplace(const char &_row,
const char &_col,
const Eigen::MatrixXd &_mat_blk);
// throw error if queried block is not present
bool at(const char &_row,
const char &_col,
Eigen::MatrixXd &_mat_blk) const;
const MatrixXd& at(const char &_row,
const char &_col) const;
MatrixXd& at(const char &_row,
const char &_col);
// make some shadowed API available
using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::at;
using std::unordered_map < char, std::unordered_map < char, Eigen::MatrixXd > >::count;
MatrixXd matrix(const StateStructure & _struct_rows,
const StateStructure & _struct_cols) const;
MatrixComposite operator * (double scalar_right) const;
MatrixComposite operator + (const MatrixComposite & _second) const;
MatrixComposite operator - (const MatrixComposite & _second) const;
MatrixComposite operator - (void) const;
/**
* \brief Matrix product
*
* This function computes the matrix product M * N
*
* It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
* both in their structure and their individual sizes.
*
* For example: Let us call 'this' matrix with the name 'M'.
*
* The matrix 'M' maps the "PO" space into a new space "VW":
* M["V"]["P"] M["V"]["O"]
* M["W"]["P"] M["W"]["O"]
*
* The second matrix N has blocks "PO" in vertical arrangement,
* and "XY" in horizontal arrangement:
* N["P"]["X"] N["P"]["Y"]
* N["O"]["X"] N["O"]["Y"]
*
* Also, assume that the sizes of the blocks "P" and "O" are the same in M and N.
*
* Then, the result is a matrix S = M * N with block arrangements "VW" and "XY"
* S["V"]["X"] S["V"]["Y"]
* S["W"]["X"] S["W"]["Y"]
*/
MatrixComposite operator *(const MatrixComposite & _second) const;
/**
* \brief Propagate a given covariances matrix, with 'this' being a Jacobian matrix
*
* This function computes the product J * Q * J.transpose.
*
* It considers that this is a Jacobian matrix, and that the provided matrix
* is a proper covariance (e.g. symmmetric and semi-positive).
* It also considers that the Jacobian blocks are compatible with the blocks
* of the provided covariance, both in their structure and their individual sizes.
*
* If these conditions are not satisfied, the product will fail and throw,
* or give aberrant results at best.
*
* -----
*
* For example: Let us call 'this' a Jacobian matrix with the name 'J'.
*
* This Jacobian 'J' maps the "PO" blocks into "VW":
*
* J["V"]["P"] J["V"]["O"]
* J["W"]["P"] J["W"]["O"]
*
* The provided matrix is a covariances matrix Q.
* Q has blocks "P", "O" in both vertical and horizontal arrangements:
*
* Q["P"]["P"] Q["P"]["O"]
* Q["O"]["P"] Q["O"]["O"]
*
* Also, it is required that the sizes of the blocks "P", "O" are the same in Q and J.
*
* Now, upon a call to
*
* MatrixComposite S = J.propagate(Q);
*
* the result is a covariances matrix S with blocks "V" and "W"
*
* S["V"]["V"] S["V"]["W"]
* S["W"]["V"] S["W"]["W"]
*/
MatrixComposite propagate(const MatrixComposite & _Cov); // This performs this * _Cov * this.tr
/**
* \brief Matrix-vector product
*
* This function computes the matrix product M * N
*
* It assumes that the second matrix's blocks are compatible with the blocks of this matrix,
* both in their structure and their individual sizes.
*
* For example: Let us call 'this' matrix with the name 'M'.
*
* The matrix 'M' maps the "PO" space into a new space "VW":
* M["V"]["P"] M["V"]["O"]
* M["W"]["P"] M["W"]["O"]
*
* The second vector V has blocks "PO" in vertical arrangement,
* V["P"]
* V["O"]
*
* Also, assume that the sizes of the blocks "P" and "O" are the same in M and V.
*
* Then, the result is a vector S = M * V with block arrangement "VW"
* S["V"]
* S["W"]
*/
VectorComposite operator *(const VectorComposite & _second) const;
friend std::ostream& operator << (std::ostream & _os, const MatrixComposite & _M);
friend MatrixComposite operator * (double scalar_left, const MatrixComposite& M);
};
class StateBlockComposite
{
public:
StateBlockComposite() = default;
~StateBlockComposite() = default;
const StateBlockMap& getStateBlockMap() const;
StateBlockPtr add(const char& _sb_type, const StateBlockPtr& _sb);
// Emplace derived state blocks (angle, quaternion, etc).
template<typename SB, typename ... Args>
std::shared_ptr<SB> emplace(const char& _sb_type, Args&&... _args_of_derived_state_block_constructor);
unsigned int remove(const char& _sb_type);
bool has(const char& _sb_type) const { return state_block_map_.count(_sb_type) > 0; }
bool has(const StateBlockPtr& _sb) const;
StateBlockPtr at(const char& _sb_type) const;
void set(const char& _sb_type, const StateBlockPtr& _sb);
void set(const char& _sb_type, const VectorXd& _vec);
void setVectors(const StateStructure& _structure, const std::list<VectorXd> &_vectors);
bool key(const StateBlockPtr& _sb, char& _key) const;
char key(const StateBlockPtr& _sb) const;
StateBlockMapCIter find(const StateBlockPtr& _sb) const;
// identity and zero (they are the same with different names)
void setIdentity(bool _notify = true);
void setZero(bool _notify = true);
VectorComposite identity() const;
VectorComposite zero() const;
// Plus operator
void plus(const VectorComposite& _dx);
/**
* \brief perturb all states states with random noise
* following an uniform distribution in [ -amplitude, amplitude ]
*/
void perturb(double amplitude = 0.01);
// 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.
void fix();
void unfix();
bool isFixed() const;
// Get composite of state vectors (not blocks)
VectorComposite getState() const;
bool getState(VectorComposite& _state) const;
void setState(const VectorComposite& _state);
// Get compact Eigen vector and related things
SizeEigen stateSize() const;
SizeEigen stateSize(const StateStructure& _structure) const;
VectorXd stateVector(const StateStructure& _structure) const;
void stateVector(const StateStructure& _structure, VectorXd& _vector) const;
private:
StateBlockMap state_block_map_;
};
//////////// IMPLEMENTATION ////////////
inline unsigned int MatrixComposite::count(const char &_row, const char &_col) const
{
const auto& rit = this->find(_row);
if (rit == this->end())
return 0;
else
return rit->second.count(_col);
}
template<typename SB, typename ... Args>
inline std::shared_ptr<SB> wolf::StateBlockComposite::emplace(const char &_sb_type,
Args &&... _args_of_derived_state_block_constructor)
{
assert(state_block_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)...);
add(_sb_type, sb);
return sb;
}
}
#endif /* STATE_BLOCK_STATE_COMPOSITE_H_ */
include/core/state_block/state_homogeneous_3d.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \file state_homogeneous_3d.h
*
* Created on: Mar 7, 2016
* \author: jsola
*/
#ifndef SRC_STATE_HOMOGENEOUS_3d_H_
#define SRC_STATE_HOMOGENEOUS_3d_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_homogeneous.h"
namespace wolf {
namespace wolf
{
class StateHomogeneous3d : public StateBlock
{
public:
StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
~StateHomogeneous3d() override;
public:
StateHomogeneous3d(bool _fixed = false, bool _transformable = true);
StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed = false, bool _transformable = true);
~StateHomogeneous3d() override;
static Eigen::VectorXd Identity();
WOLF_STATE_BLOCK_CREATE(StateHomogeneous3d);
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
virtual void transform(const VectorComposite& _transformation) override;
Eigen::Vector3d point() const;
void setStatePoint(const Eigen::Vector3d&);
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
virtual void transform(const VectorComposite& _transformation) override;
};
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable) :
StateBlock(_state, _fixed, nullptr, _transformable)
inline StateHomogeneous3d::StateHomogeneous3d(const Eigen::VectorXd _state, bool _fixed, bool _transformable)
: StateBlock("StateHomogeneous3d", _state, _fixed, nullptr, _transformable)
{
assert(_state.size() == 4 && "Homogeneous 3d must be size 4.");
if (_state.size() != 4) throw std::runtime_error("Homogeneous 3d must be size 4.");
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
}
inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable) :
StateBlock(4, _fixed, nullptr, _transformable)
inline StateHomogeneous3d::StateHomogeneous3d(bool _fixed, bool _transformable)
: StateBlock("StateHomogeneous3d", 4, _fixed, nullptr, _transformable)
{
local_param_ptr_ = std::make_shared<LocalParametrizationHomogeneous>();
state_ << 0, 0, 0, 1; // Set origin
state_ << 0, 0, 0, 1; // Set origin
}
inline StateHomogeneous3d::~StateHomogeneous3d()
......@@ -68,7 +60,6 @@ inline StateHomogeneous3d::~StateHomogeneous3d()
// The local_param_ptr_ pointer is already removed by the base class
}
inline void StateHomogeneous3d::setIdentity(bool _notify)
{
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
......@@ -79,18 +70,25 @@ inline Eigen::VectorXd StateHomogeneous3d::identity() const
return Eigen::Quaterniond::Identity().coeffs();
}
inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
inline Eigen::VectorXd StateHomogeneous3d::Identity()
{
if (transformable_)
setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs()); // TODO is this correct?????? probably not!!!
return Eigen::Quaterniond::Identity().coeffs();
}
inline Eigen::Vector3d StateHomogeneous3d::point() const
{
return state_.head<3>() / state_(3);
}
inline StateBlockPtr StateHomogeneous3d::create (const Eigen::VectorXd& _state, bool _fixed)
inline void StateHomogeneous3d::setStatePoint(const Eigen::Vector3d& _p)
{
MatrixSizeCheck<4,1>::check(_state);
return std::make_shared<StateHomogeneous3d>(_state, _fixed);
state_ << _p, 1;
}
} // namespace wolf
inline void StateHomogeneous3d::transform(const VectorComposite& _transformation)
{
if (transformable_)
setStatePoint(_transformation.at('P') + Eigen::Quaterniond(_transformation.at('O').data()) * point());
}
#endif /* SRC_STATE_HOMOGENEOUS_3d_H_ */
} // namespace wolf
include/core/state_block/state_quaternion.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
/*
* \file StateQuaternion.h
*
* Created on: Mar 7, 2016
* author: jsola
*/
#ifndef SRC_STATE_QUATERNION_H_
#define SRC_STATE_QUATERNION_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_quaternion.h"
namespace wolf {
namespace wolf
{
class StateQuaternion : public StateBlock
{
public:
StateQuaternion(bool _fixed = false, bool _transformable = true);
StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
~StateQuaternion() override;
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
virtual void transform(const VectorComposite& _transformation) override;
static StateBlockPtr create(const Eigen::VectorXd& _state, bool _fixed);
public:
StateQuaternion(bool _fixed = false, bool _transformable = true);
StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false, bool _transformable = true);
StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false, bool _transformable = true);
~StateQuaternion() override;
static Eigen::VectorXd Identity();
WOLF_STATE_BLOCK_CREATE(StateQuaternion);
void setIdentity(bool _notify = true) override;
Eigen::VectorXd identity() const override;
virtual void transform(const VectorComposite& _transformation) override;
};
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable) :
StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed, bool _transformable)
: StateBlock("StateQuaternion",
_quaternion.coeffs(),
_fixed,
std::make_shared<LocalParametrizationQuaternionLocal>(),
_transformable)
{
// TODO: remove these lines after issue #381 is addressed
assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
state_.normalize();
}
inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable) :
StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed, bool _transformable)
: StateBlock("StateQuaternion",
_state,
_fixed,
std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
_transformable)
{
assert(state_.size() == 4 && "The quaternion state vector must be of size 4");
if (state_.size() != 4) throw std::runtime_error("The quaternion state vector must be of size 4!");
// TODO: remove these lines after issue #381 is addressed
assert(isValid(1e-5) && "Quaternion unit norm is worse than 1e-5 tolerance!");
state_.normalize();
}
inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable) :
StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(), _transformable)
inline StateQuaternion::StateQuaternion(bool _fixed, bool _transformable)
: StateBlock("StateQuaternion",
4,
_fixed,
std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>(),
_transformable)
{
state_ = Eigen::Quaterniond::Identity().coeffs();
//
}
inline StateQuaternion::~StateQuaternion()
{
// The local_param_ptr_ pointer is already removed by the base class
}
inline StateQuaternion::~StateQuaternion() {}
inline void StateQuaternion::setIdentity(bool _notify)
{
setState(Eigen::Quaterniond::Identity().coeffs(), _notify);
}
inline Eigen::VectorXd StateQuaternion::identity() const
inline Eigen::VectorXd StateQuaternion::Identity()
{
return Eigen::Quaterniond::Identity().coeffs();
}
inline void StateQuaternion::transform(const VectorComposite& _transformation)
inline Eigen::VectorXd StateQuaternion::identity() const
{
if (transformable_)
setState((Quaterniond(_transformation.at('O').data()) * Quaterniond(getState().data())).coeffs());
return Eigen::Quaterniond::Identity().coeffs();
}
inline StateBlockPtr StateQuaternion::create (const Eigen::VectorXd& _state, bool _fixed)
inline void StateQuaternion::transform(const VectorComposite& _transformation)
{
MatrixSizeCheck<4,1>::check(_state);
return std::make_shared<StateQuaternion>(_state, _fixed);
if (transformable_)
setState(
(Eigen::Quaterniond(_transformation.at('O').data()) * Eigen::Quaterniond(getState().data())).coeffs());
}
} // namespace wolf
#endif /* SRC_STATE_QUATERNION_H_ */
} // namespace wolf
include/core/trajectory/trajectory_base.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef TRAJECTORY_BASE_H_
#define TRAJECTORY_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Fwd refs
namespace wolf{
namespace wolf
{
class Problem;
class FrameBase;
}
} // namespace wolf
//Wolf includes
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/time_stamp.h"
#include "core/state_block/state_composite.h"
namespace wolf {
#include "core/composite/vector_composite.h"
namespace wolf
{
class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<TrajectoryBase>
{
friend FrameBase;
private:
FramePtrMap frame_map_;
protected:
StateStructure frame_structure_; // Defines the structure of the Frames in the Trajectory.
public:
TrajectoryBase();
~TrajectoryBase() override;
// Frames
SizeEigen size() const;
FrameConstPtrMap getFrameMap() const;
FramePtrMap getFrameMap();
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
FrameBaseConstPtr getFirstFrame() const;
FrameBasePtr getFirstFrame();
FrameBaseConstPtr getNextFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
FrameBasePtr getNextFrame(FrameBasePtr, const unsigned int& i = 1);
FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
FrameBasePtr getPreviousFrame(FrameBasePtr, const unsigned int& i = 1);
TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
bool hasTimeStamp(const TimeStamp& _ts) const;
bool hasFrame(FrameBaseConstPtr _frame) const;
virtual void printHeader(int depth, //
bool constr_by, //
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, TrajectoryBaseConstPtr _trj_ptr, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, NodeBaseConstPtr _node_ptr, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
public:
// factors
void getFactorList(FactorBaseConstPtrList & _fac_list) const;
void getFactorList(FactorBasePtrList & _fac_list);
private:
FramePtrMap frame_map_;
protected:
StateKeys frame_structure_; // Defines the structure of the Frames in the Trajectory.
public:
TrajectoryBase();
~TrajectoryBase() override;
bool hasChildren() const override;
// Frames
SizeEigen size() const;
FrameConstPtrMap getFrameMap() const;
FramePtrMap getFrameMap();
FrameBaseConstPtr getLastFrame() const;
FrameBasePtr getLastFrame();
FrameBaseConstPtr getFirstFrame() const;
FrameBasePtr getFirstFrame();
FrameBaseConstPtr getNextFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
FrameBasePtr getNextFrame(FrameBasePtr, const unsigned int& i = 1);
FrameBaseConstPtr getPreviousFrame(FrameBaseConstPtr, const unsigned int& i = 1) const;
FrameBasePtr getPreviousFrame(FrameBasePtr, const unsigned int& i = 1);
TimeStamp closestTimeStampToTimeStamp(const TimeStamp& _ts) const;
FrameBaseConstPtr closestFrameToTimeStamp(const TimeStamp& _ts) const;
FrameBasePtr closestFrameToTimeStamp(const TimeStamp& _ts);
bool hasTimeStamp(const TimeStamp& _ts) const;
bool hasFrame(FrameBaseConstPtr _frame) const;
virtual void printHeader(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream,
std::string _tabs = "") const;
void print(int depth, //
bool factored_by, //
bool metric, //
bool state_blocks,
std::ostream& stream = std::cout,
std::string _tabs = "") const;
virtual CheckLog localCheck(bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
bool check(CheckLog& _log, bool _verbose, std::ostream& _stream, std::string _tabs = "") const;
private:
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
void removeFrame(FrameBasePtr _frame_ptr);
public:
// factors
void getFactorList(FactorBaseConstPtrList& _fac_list) const;
void getFactorList(FactorBasePtrList& _fac_list);
};
inline bool TrajectoryBase::hasChildren() const
{
return not frame_map_.empty();
}
inline FrameConstPtrMap TrajectoryBase::getFrameMap() const
{
FrameConstPtrMap map_const;
for (auto&& pair : frame_map_)
map_const[pair.first] = pair.second;
for (auto&& pair : frame_map_) map_const[pair.first] = pair.second;
return map_const;
}
......@@ -109,29 +111,25 @@ inline FramePtrMap TrajectoryBase::getFrameMap()
inline FrameBaseConstPtr TrajectoryBase::getFirstFrame() const
{
if (frame_map_.empty())
return nullptr;
if (frame_map_.empty()) return nullptr;
return frame_map_.begin()->second;
}
inline FrameBasePtr TrajectoryBase::getFirstFrame()
{
if (frame_map_.empty())
return nullptr;
if (frame_map_.empty()) return nullptr;
return frame_map_.begin()->second;
}
inline FrameBaseConstPtr TrajectoryBase::getLastFrame() const
{
if (frame_map_.empty())
return nullptr;
if (frame_map_.empty()) return nullptr;
return frame_map_.rbegin()->second;
}
inline FrameBasePtr TrajectoryBase::getLastFrame()
{
if (frame_map_.empty())
return nullptr;
if (frame_map_.empty()) return nullptr;
return frame_map_.rbegin()->second;
}
......@@ -147,15 +145,11 @@ inline bool TrajectoryBase::hasTimeStamp(const TimeStamp& _ts) const
inline bool TrajectoryBase::hasFrame(FrameBaseConstPtr _frame) const
{
return std::find_if(frame_map_.begin(),
frame_map_.end(),
[&_frame](std::pair<TimeStamp,FrameBaseConstPtr> frm_it)
{
return std::find_if(frame_map_.cbegin(),
frame_map_.cend(),
[&_frame](const std::pair<TimeStamp, FrameBaseConstPtr>& frm_it) {
return frm_it.second == _frame;
}) != frame_map_.end();
}
} // namespace wolf
#endif
} // namespace wolf
include/core/tree_manager/factory_tree_manager.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef FACTORY_TREE_MANAGER_H_
#define FACTORY_TREE_MANAGER_H_
namespace wolf
{
class TreeManagerBase;
struct ParamsTreeManagerBase;
}
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// wolf
#include "core/common/factory.h"
// std
#include "yaml-cpp/yaml.h"
namespace wolf
{
/** \brief TreeManager factory class
* TODO
*/
// ParamsTreeManager factory
struct ParamsTreeManagerBase;
typedef Factory<ParamsTreeManagerBase,
const std::string&> FactoryParamsTreeManager;
template<>
inline std::string FactoryParamsTreeManager::getClass() const
typedef Factory<std::shared_ptr<TreeManagerBase>, const YAML::Node&, const std::vector<std::string>&>
FactoryTreeManagerNode;
template <>
inline std::string FactoryTreeManagerNode::getClass() const
{
return "FactoryParamsTreeManager";
return "FactoryTreeManagerNode";
}
// TreeManager factory
typedef Factory<TreeManagerBase,
const std::string&,
const ParamsTreeManagerBasePtr> FactoryTreeManager;
template<>
inline std::string FactoryTreeManager::getClass() const
typedef Factory<std::shared_ptr<TreeManagerBase>, const std::string&, const std::vector<std::string>&>
FactoryTreeManagerFile;
template <>
inline std::string FactoryTreeManagerFile::getClass() const
{
return "FactoryTreeManager";
return "FactoryTreeManagerFile";
}
#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
namespace{ const bool WOLF_UNUSED TreeManagerType##Registered = \
wolf::FactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
typedef Factory<TreeManagerBase,
const std::string&,
const ParamsServer&> AutoConfFactoryTreeManager;
template<>
inline std::string AutoConfFactoryTreeManager::getClass() const
{
return "AutoConfFactoryTreeManager";
}
#define WOLF_REGISTER_TREE_MANAGER_AUTO(TreeManagerType) \
namespace{ const bool WOLF_UNUSED TreeManagerType##AutoConfRegistered = \
wolf::AutoConfFactoryTreeManager::registerCreator(#TreeManagerType, TreeManagerType::create); } \
#define WOLF_REGISTER_TREE_MANAGER(TreeManagerType) \
namespace \
{ \
const bool WOLF_UNUSED TreeManagerType##Registered = \
wolf::FactoryTreeManagerNode::registerCreator(#TreeManagerType, TreeManagerType::create); \
} \
namespace \
{ \
const bool WOLF_UNUSED TreeManagerType##YamlRegistered = \
wolf::FactoryTreeManagerFile::registerCreator(#TreeManagerType, TreeManagerType::create); \
}
} /* namespace wolf */
#endif /* FACTORY_TREE_MANAGER_H_ */
include/core/tree_manager/tree_manager_base.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef INCLUDE_TREE_MANAGER_BASE_H_
#define INCLUDE_TREE_MANAGER_BASE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
// Wolf includes
#include "core/common/wolf.h"
#include "core/common/node_base.h"
#include "core/common/params_base.h"
#include "core/problem/problem.h"
#include "core/tree_manager/factory_tree_manager.h"
namespace wolf
{
......@@ -39,65 +35,52 @@ namespace wolf
* In order to use this macro, the derived processor class, ProcessorClass,
* must have a constructor available with the API:
*
* TreeManagerClass(const ParamsTreeManagerPtr _params);
* TreeManagerClass(const YAML::Node& _params);
*
* Also, there should be the schema file 'TreeManagerClass.schema' containing the specifications
* of the user input yaml file.
*/
#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass, ParamsTreeManagerClass) \
static TreeManagerBasePtr create(const std::string& _unique_name, \
const ParamsServer& _server) \
{ \
auto params = std::make_shared<ParamsTreeManagerClass>(_unique_name, _server); \
\
auto tree_manager = std::make_shared<TreeManagerClass>(params); \
\
tree_manager ->setName(_unique_name); \
\
return tree_manager; \
} \
static TreeManagerBasePtr create(const std::string& _unique_name, \
const ParamsTreeManagerBasePtr _params) \
{ \
auto params = std::static_pointer_cast<ParamsTreeManagerClass>(_params); \
\
auto tree_manager = std::make_shared<TreeManagerClass>(params); \
\
tree_manager ->setName(_unique_name); \
\
return tree_manager; \
} \
struct ParamsTreeManagerBase : public ParamsBase
{
std::string prefix = "problem/tree_manager";
ParamsTreeManagerBase() = default;
ParamsTreeManagerBase(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
{
#define WOLF_TREE_MANAGER_CREATE(TreeManagerClass) \
static TreeManagerBasePtr create(const YAML::Node& _input_node, \
const std::vector<std::string>& _folders_schema = {}) \
{ \
if (not _folders_schema.empty()) \
{ \
auto server = yaml_schema_cpp::YamlServer(_folders_schema); \
server.setYaml(_input_node); \
if (not server.applySchema(#TreeManagerClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
} \
return std::shared_ptr<TreeManagerClass>(new TreeManagerClass(_input_node)); \
} \
static TreeManagerBasePtr 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(#TreeManagerClass)) \
{ \
WOLF_ERROR(server.getLog()); \
return nullptr; \
} \
return create(server.getNode(), {}); \
}
~ParamsTreeManagerBase() override = default;
std::string print() const override
{
return "";
}
};
class TreeManagerBase : public NodeBase
{
public:
TreeManagerBase(const std::string& _type, ParamsTreeManagerBasePtr _params) :
NodeBase("TREE_MANAGER", _type),
params_(_params)
{}
public:
TreeManagerBase(const std::string& _type, const YAML::Node& _params) : NodeBase("TREE_MANAGER", _type) {}
virtual ~TreeManagerBase() {}
virtual ~TreeManagerBase() {}
virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
virtual void keyFrameCallback(FrameBasePtr _key_frame) = 0;
protected:
ParamsTreeManagerBasePtr params_;
bool hasChildren() const override
{
return true;
};
};
} /* namespace wolf */
#endif /* INCLUDE_TREE_MANAGER_BASE_H_ */
include/core/tree_manager/tree_manager_sliding_window.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/tree_manager/tree_manager_base.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindow)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindow)
struct ParamsTreeManagerSlidingWindow : public ParamsTreeManagerBase
{
ParamsTreeManagerSlidingWindow() = default;
ParamsTreeManagerSlidingWindow(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsTreeManagerBase(_unique_name, _server)
{
n_frames = _server.getParam<unsigned int>(prefix + "/n_frames");
n_fix_first_frames = _server.getParam<unsigned int>(prefix + "/n_fix_first_frames");
viral_remove_empty_parent = _server.getParam<bool> (prefix + "/viral_remove_empty_parent");
if (n_frames <= n_fix_first_frames)
throw std::runtime_error("TreeManagerSlidingWindow: Wrong parameter value. 'n_fix_first_frames' should be lower than 'n_frames'!");
}
std::string print() const override
{
return ParamsTreeManagerBase::print() + "\n"
+ "n_frames: " + std::to_string(n_frames) + "\n"
+ "n_fix_first_frames: " + std::to_string(n_fix_first_frames) + "\n"
+ "viral_remove_empty_parent: " + std::to_string(viral_remove_empty_parent) + "\n";
}
unsigned int n_frames;
unsigned int n_fix_first_frames;
bool viral_remove_empty_parent;
};
class TreeManagerSlidingWindow : public TreeManagerBase
{
public:
TreeManagerSlidingWindow(ParamsTreeManagerSlidingWindowPtr _params) :
TreeManagerBase("TreeManagerSlidingWindow", _params),
params_sw_(_params)
{};
WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow, ParamsTreeManagerSlidingWindow)
public:
TreeManagerSlidingWindow(const YAML::Node& _params);
WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindow)
~TreeManagerSlidingWindow() override{}
~TreeManagerSlidingWindow() override {}
void keyFrameCallback(FrameBasePtr _frame) override;
void keyFrameCallback(FrameBasePtr _frame) override;
protected:
ParamsTreeManagerSlidingWindowPtr params_sw_;
protected:
// PARAMS
unsigned int n_frames_;
unsigned int n_fix_first_frames_;
bool viral_remove_parent_without_children_;
};
} /* namespace wolf */
#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_H_ */
include/core/tree_manager/tree_manager_sliding_window_dual_rate.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
#define INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/tree_manager/tree_manager_sliding_window.h"
namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ParamsTreeManagerSlidingWindowDualRate)
WOLF_PTR_TYPEDEFS(TreeManagerSlidingWindowDualRate)
struct ParamsTreeManagerSlidingWindowDualRate : public ParamsTreeManagerSlidingWindow
class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
ParamsTreeManagerSlidingWindowDualRate() = default;
ParamsTreeManagerSlidingWindowDualRate(std::string _unique_name, const wolf::ParamsServer & _server) :
ParamsTreeManagerSlidingWindow(_unique_name, _server)
{
n_frames_recent = _server.getParam<unsigned int>(prefix + "/n_frames_recent");
assert(n_frames_recent <= n_frames);
rate_old_frames = _server.getParam<unsigned int>(prefix + "/rate_old_frames");
}
std::string print() const override
{
return ParamsTreeManagerBase::print() + "\n"
+ "n_frames_recent: " + std::to_string(n_frames_recent) + "\n"
+ "rate_old_frames: " + std::to_string(rate_old_frames) + "\n";
}
public:
TreeManagerSlidingWindowDualRate(const YAML::Node& _params);
unsigned int n_frames_recent, rate_old_frames;
};
WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate)
class TreeManagerSlidingWindowDualRate : public TreeManagerSlidingWindow
{
public:
TreeManagerSlidingWindowDualRate(ParamsTreeManagerSlidingWindowDualRatePtr _params);
;
WOLF_TREE_MANAGER_CREATE(TreeManagerSlidingWindowDualRate, ParamsTreeManagerSlidingWindowDualRate)
~TreeManagerSlidingWindowDualRate() override {}
~TreeManagerSlidingWindowDualRate() override{}
void keyFrameCallback(FrameBasePtr _frame) override;
void keyFrameCallback(FrameBasePtr _frame) override;
protected:
unsigned int count_frames_; ///< counter of frames
protected:
ParamsTreeManagerSlidingWindowDualRatePtr params_swdr_;
unsigned int count_frames_;
//TrajectoryIter first_recent_frame_it_;
// PARAMS
unsigned int n_frames_recent_;
unsigned int rate_old_frames_;
};
} /* namespace wolf */
#endif /* INCLUDE_TREE_MANAGER_SLIDING_WINDOW_DUAL_RATE_H_ */
include/core/utils/check_log.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef CHECK_LOG_H
#define CHECK_LOG_H
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include <iostream>
#include <string>
#include <sstream>
......@@ -39,5 +36,5 @@ class CheckLog
void compose(CheckLog l);
void assertTrue(bool _condition, std::stringstream& _stream);
};
} // namespace wolf
#endif
include/core/utils/converter.h deleted 100644 → 0
View file @ c7bb5751
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef CONVERTER_H
#define CONVERTER_H
// wolf
#include "core/common/wolf.h"
#include "core/utils/converter_utils.h"
#include "core/state_block/state_composite.h"
// Eigen
#include <Eigen/Dense>
#include <Eigen/Geometry>
// std
#include <regex>
#include <iostream>
#include <array>
#include <vector>
#include <stack>
#include <list>
#include <math.h>
/**
@file
converter.h
@brief This file implements a set of simple functions that deal with the correspondence between
classes and their string representation. The YAML autosetup framework makes heavy use of this file.
*/
//Note: In order to deal with string representations we make heavy use of regexps.
// We use the standard C++11 regular expressions capabilities.
/*
** This file is structured essentially in two parts:
** in the first part (which starts after the CONVERTERS ~~~~ STRING ----> TYPE line)
** we have functions to convert from string to C++ class. For example:
string v1 = "[3,4,5,6,7,8,9,10,11]";
vector<int> v = converter<vector<int>>::convert(v1);
This code snippet transforms the string v1 which represents an std::vector into an actual std::vector value.
The second part (which starts after the TYPES ----> ToSTRING line) has the functions to
transform from C++ classes to strings. For instance, if we wanted to convert from a C++ class
to a string we would do something like this:
vector<int> vect{ 10, 20, 30 };
string str = converter<std::string>::convert(vect);
//str == "[10,20,30]"
*/
namespace wolf{
//This definition is a bit of a "hack". The reason of redefining the pair class is to be able
//to have two string representations of a pair, namely
//"(·,·)" -> typical pair/tuple representation
//"{·,·}" -> key-value pair representation used to represent maps.
//This is purely an aesthetic reason and could be removed without problems.
template<class A, class B>
struct Wpair : std::pair<A,B>
{
Wpair(A first, B second): std::pair<A,B>(first, second)
{
}
};
//// CONVERTERS ~~~~ STRING ----> TYPE
template<typename T>
struct converter{
static T convert(std::string val){
throw std::runtime_error("There is no general convert for arbitrary T !!! String provided: " + val);
}
};
template<typename A>
struct converter<utils::list<A>>{
static utils::list<A> convert(std::string val){
std::regex rgxP("\\[([^,]+)?(,[^,]+)*\\]");
utils::list<A> result = utils::list<A>();
if(std::regex_match(val, rgxP)) {
// std::string aux = val.substr(1,val.size()-2);
// auto l = utils::getMatches(aux, std::regex("([^,]+)"));
auto l = utils::parseList(val);
for(auto it : l){
// WOLF_DEBUG("Asking to convert in list ", it);
result.push_back(converter<A>::convert(it));
}
} else throw std::runtime_error("Invalid string format representing a list-like structure. Correct format is [(value)?(,value)*]. String provided: " + val);
return result;
}
static std::string convert(utils::list<A> val){
std::string aux = "";
bool first = true;
for(auto it : val){
if(not first) aux += "," + converter<A>::convert(it);
else{
first = false;
aux = converter<A>::convert(it);
}
}
return "[" + aux + "]";
}
};
template<>
struct converter<int>{
static int convert(std::string val){
double res;
if (modf(stod(val),&res) > 0)
throw std::runtime_error("Invalid conversion to int: The number contains decimals: " + val);
return res;
}
};
template<>
struct converter<unsigned int>{
static unsigned int convert(std::string val){
double res;
if (modf(stod(val),&res) > 0)
throw std::runtime_error("Invalid conversion to unsigned int: The number contains decimals: " + val);
if (res < 0)
throw std::runtime_error("Invalid conversion to unsigned int: The number is negative: " + val);
return res;
}
};
template<>
struct converter<double>{
static double convert(std::string val){
return stod(val);
}
};
template<>
struct converter<bool>{
static bool convert(std::string val){
if(val == "true" or val=="True" or val=="TRUE") return true;
else if (val == "false" or val=="False" or val=="FALSE") return false;
else throw std::runtime_error("Invalid conversion to bool (Must be either \"true\" or \"false\"). String provided: " + val);
}
};
template<>
struct converter<char>{
static char convert(std::string val){
//Here we should check that val.length() == 1 and get val[0] into a char variable
if(val.length() == 1) return val.at(0);
else throw std::runtime_error("Invalid converstion to char. String too long. String provided: " + val);
throw std::runtime_error("Invalid char conversion. String provided: " + val);
}
};
//// TYPES ----> ToSTRING
template<>
struct converter<std::string>{
static std::string convert(std::string val){
return val;
}
template<typename T>
static std::string convert(T val){
// throw std::runtime_error("There is no general convert to string for arbitrary T !!! String provided: " + val);
throw std::runtime_error("There is no general convert to string for arbitrary T !!!");
}
static std::string convert(int val){
return std::to_string(val);
}
static std::string convert(double val){
return std::to_string(val);
}
static std::string convert(char val){
return std::to_string(val);
}
template<typename A>
static std::string convert(utils::list<A> val){
std::string result = "";
for(auto it : val){
result += "," + converter<std::string>::convert(it);
}
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
template <typename A> static std::string convert(std::list<A> val) {
std::string result = "";
for (auto it : val) {
result += "," + converter<std::string>::convert(it);
}
if (result.size() > 0) result = result.substr(1, result.size());
return "[" + result + "]";
}
template<typename A, typename B>
static std::string convert(std::pair<A,B> val){
return "{" + converter<std::string>::convert(val.first) + ":" + converter<std::string>::convert(val.second) + "}";
}
//NEW GUY
template<typename A, typename B>
static std::string convert(Wpair<A,B> val){
return "(" + converter<std::string>::convert(val.first) + "," + converter<std::string>::convert(val.second) + ")";
}
template<typename A, typename B>
static std::string convert(std::map<A,B> val){
std::string result = "";
for(auto it : val){
result += "," + converter<std::string>::convert(it);
}
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
template<typename A, typename B>
static std::string convert(std::unordered_map<A,B> val){
std::string result = "";
for(auto it : val){
result += "," + converter<std::string>::convert(it);
}
if(result.size() > 0) result = result.substr(1,result.size());
return "[" + result + "]";
}
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
static std::string convert(Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> val){
std::string result = "";
for(int i = 0; i < val.rows() ; ++i){
for(int j = 0; j < val.cols(); ++j){
result += "," + converter<std::string>::convert(val(i,j));
}
}
if(result.size() > 0) result = result.substr(1,result.size());
if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic){
result = "[" + std::to_string(val.rows()) + "," + std::to_string(val.cols()) + "]," + result;
}
return "[" + result + "]";
}
//// WOLF DEFINED TYPES -----> TO STRING
static std::string convert(VectorComposite val){
//TODO
// throw std::runtime_error("TODO! We still haven't got around to implement convert for VectorComposite to String :(");
auto helper = std::unordered_map<char, Eigen::VectorXd>();
for(const auto& pair: val)
helper.insert(std::pair<char, Eigen::VectorXd>(pair.first, pair.second));
return converter<std::string>::convert(helper);
}
static std::string convert(MatrixComposite val){
//TODO
// throw std::runtime_error("TODO! We still haven't got around to implement convert for MatrixComposite to String :(");
auto helper = std::unordered_map< char, std::unordered_map<char, Eigen::MatrixXd>>();
for(const auto& pair: val)
helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(pair.first, pair.second));
return converter<std::string>::convert(helper);
}
};
//// CONVERTERS ~~~~ TYPE ----> STRING
template<typename A, typename B>
struct converter<std::pair<A,B>>{
static std::pair<A,B> convert(std::string val){
std::regex rgxP("\\{([^\\{:]+):([^\\}]+)\\}");
std::smatch matches;
if(std::regex_match(val, matches, rgxP)) {
return std::pair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a pair. Correct format is {identifier:value}. String provided: " + val);
}
};
//NEW GUY
template<typename A, typename B>
struct converter<Wpair<A,B>>{
static Wpair<A,B> convert(std::string val){
std::regex rgxP("\\(([^\\(,]+),([^\\)]+)\\)");
std::smatch matches;
if(std::regex_match(val, matches, rgxP)) {
return Wpair<A,B>(converter<A>::convert(matches[1].str()), converter<B>::convert(matches[2].str()));
} else throw std::runtime_error("Invalid string format representing a Wpair. Correct format is (first,second). String provided: " + val);
}
};
// TODO: WARNING!! For some reason when trying to specialize converter to std::array
// it defaults to the generic T type, thus causing an error!
template<typename A, unsigned int N>
struct converter<std::array<A, N>>{
static std::array<A,N> convert(std::string val){
// std::vector<std::string> aux = utils::splitter(val);
auto aux = converter<utils::list<A>>::convert(val);
std::array<A,N> rtn = std::array<A,N>();
if(N != aux.size()) throw std::runtime_error("Error in trying to transform literal string to Array. Invalid argument size. Required size "
+ std::to_string(N) + "; provided size " + std::to_string(aux.size()));
for (int i = 0; i < N; ++i) {
rtn[i] = aux[i];
}
return rtn;
}
};
template<typename _Scalar, int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct converter<Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>>{
typedef Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols> Matrix;
static Matrix convert(std::string val){
auto splittedValues = utils::splitMatrixStringRepresentation(val);
auto dimensions = converter<std::vector<int>>::convert(splittedValues[0]);
auto values = converter<std::vector<_Scalar>>::convert(splittedValues[1]);
Matrix m = Matrix();
if(_Rows == Eigen::Dynamic and _Cols == Eigen::Dynamic) {
if(dimensions.size() != 2) throw std::runtime_error("Invalid string representing a dynamic Eigen Matrix. Missing dimensions. String provided: " + val);
m.resize(dimensions[0],dimensions[1]);
}else if(_Rows == Eigen::Dynamic){
int nrows = (int) values.size() / _Cols;
m.resize(nrows, _Cols);
}else if(_Cols == Eigen::Dynamic){
int ncols = (int) values.size() / _Rows;
m.resize(_Rows, ncols);
}
if(m.rows()*m.cols() != (int) values.size()) throw std::runtime_error("The literal string provides " + std::to_string(values.size()) + " values but "
+ "the Eigen matrix is of dimensions "
+ std::to_string(m.rows()) + "x" + std::to_string(m.cols()));
else{
for (int i = 0; i < m.rows(); i++)
for (int j = 0; j < m.cols(); j++)
m(i, j) = values[(int)(i * m.cols() + j)];
}
return m;
}
};
template<typename A>
struct converter<std::map<std::string,A>>{
static std::map<std::string,A> convert(std::string val){
std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
if(not std::regex_match(val, rgxM))
throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
auto v = utils::parseList(val);
auto map = std::map<std::string, A>();
for(auto it : v){
auto p = converter<std::pair<std::string, A>>::convert(it);
map.insert(std::pair<std::string, A>(p.first, p.second));
}
return map;
}
};
template<typename A, typename B>
struct converter<std::map<A,B>>{
static std::map<A,B> convert(std::string val){
std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
if(not std::regex_match(val, rgxM))
throw std::runtime_error("Invalid string representation of a Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
auto v = utils::parseList(val);
auto map = std::map<A, B>();
for(auto it : v){
auto p = converter<std::pair<A,B>>::convert(it);
map.insert(std::pair<A, B>(p.first, p.second));
}
return map;
}
};
template<typename A, typename B>
struct converter<std::unordered_map<A,B>>{
static std::unordered_map<A,B> convert(std::string val){
std::regex rgxM("\\[((?:(?:\\{[^\\{:]+:[^:\\}]+\\}),?)*)\\]");
if(not std::regex_match(val, rgxM))
throw std::runtime_error("Invalid string representation of an unordered Map. Correct format is [({id:value})?(,{id:value})*]. String provided: " + val);
auto v = utils::parseList(val);
auto map = std::unordered_map<A, B>();
for(auto it : v){
auto p = converter<std::pair<A,B>>::convert(it);
map.insert(std::pair<A, B>(p.first, p.second));
}
return map;
}
};
//// FROM STRING -----> WOLF DEFINED TYPES
template<>
struct converter<VectorComposite>{
static VectorComposite convert(std::string val){
auto unordered_map = converter<std::unordered_map<char, Eigen::VectorXd>>::convert(val);
// VectorComposite result = VectorComposite(unordered_map);
// return result;
auto helper = VectorComposite();
for(auto const& it: unordered_map)
{
helper.insert(std::pair<char, Eigen::VectorXd>(it.first, it.second));
}
return helper;
}
};
template<>
struct converter<MatrixComposite>{
static MatrixComposite convert(std::string val){
auto unordered_map = converter<std::unordered_map<char,
std::unordered_map<char, Eigen::MatrixXd>>>::convert(val);
// VectorComposite result = VectorComposite(unordered_map);
// return result;
auto helper = MatrixComposite();
for(auto const& it: unordered_map)
{
helper.insert(std::pair<char, std::unordered_map<char, Eigen::MatrixXd>>(it.first, it.second));
}
return helper;
}
};
}
#endif
include/core/utils/converter_utils.h deleted 100644 → 0
View file @ c7bb5751
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef CONVERTER_UTILS_H
#define CONVERTER_UTILS_H
// wolf
#include "core/common/wolf.h"
// std
#include <regex>
namespace utils{
//Typically we want to convert from/to list-type structures. In order to be general
//we define a list type which is used throughout the converter. In this case this type
//is implemented with std::vector
template <typename A>
using list = std::vector<A>;
// template <typename A>
// class toString<A>{};
/** @Brief Splits a comma-separated string into its pieces
* @param val is just the string of comma separated values
* @return <b>{std::vector<std::string>}</b> vector whose i-th component is the i-th comma separated value
*/
std::vector<std::string> splitter(std::string val);
/** @Brief Returns all the substrings of @val that match @exp
* @param val String to be matched
* @param exp Regular expression
* @return <b>{std::vector<std::string>}</b> Collection of matching substrings
*/
std::vector<std::string> getMatches(std::string val, std::regex exp);
/** @Brief Given a string representation of a matrix extracts the dimensions and the values
* @param matrix is a string either of the form "[[N,M],[a1,a2,a3,...]]" or "[a1,a2,a3,...]"
* @return <b>{std::array<std::string, 2>}</b> pair ([N,M],[a1,a2,a3,...]) or just ([a1,a2,a3...])
*/
std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix);
/** @Brief Splits a dictionary-like string of the form {k1:v1},{k2:v2},... It is tightly coupled with splitMapStringRepresentation
* @param val is just a dictionary-like string
* @return <b>{std::vector<std::string>}</b> Collection of the strings of the form {k_i:v_i}
*/
std::vector<std::string> pairSplitter(std::string val);
/** @Brief Splits a dictionary-like string of the form [{k1:v1},{k2:v2},...]
* @param str_map just a dictionary-like string
* @return <b>{std::string}</b> String {k1:v1},{k2:v2},... (notice the removed brackets)
*/
std::string splitMapStringRepresentation(std::string str_map);
std::vector<std::string> parseList(std::string val);
}
#endif
include/core/utils/eigen_assert.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef _WOLF_EIGEN_ASSERT_H_
#define _WOLF_EIGEN_ASSERT_H_
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include <Eigen/Dense>
namespace Eigen {
namespace Eigen
{
//////////////////////////////////////////////////////////
/** Check Eigen Matrix sizes, both statically and dynamically
*
......@@ -34,7 +30,8 @@ namespace Eigen {
* The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's
* StaticAssert.h):
*
* EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE
* EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE
......@@ -42,7 +39,8 @@ namespace Eigen {
*
* but they do not work if the evaluated types are of dynamic size.
*
* In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this kind:
* In order to achieve full templatization over both dynamic and static sizes, we use extensively a prototype of this
* kind:
*
* template<typename Derived>
* inline Eigen::Matrix<typename Derived::Scalar, 3, 3> function(const Eigen::MatrixBase<Derived>& _v){
......@@ -59,44 +57,44 @@ namespace Eigen {
* The function : MatrixSizeCheck <Rows, Cols>::check(M) checks that the Matrix M is of size ( Rows x Cols ).
* This check is performed statically or dynamically, depending on the type of argument provided.
*/
template<int Size, int DesiredSize>
template <int Size, int DesiredSize>
struct StaticDimCheck
{
template<typename T>
StaticDimCheck(const T&)
{
static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
}
template <typename T>
StaticDimCheck(const T&)
{
static_assert(Size == DesiredSize, "Size of static Vector or Matrix does not match");
}
};
template<int DesiredSize>
template <int DesiredSize>
struct StaticDimCheck<Eigen::Dynamic, DesiredSize>
{
template<typename T>
StaticDimCheck(const T& t)
{
if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl;
assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
}
template <typename T>
StaticDimCheck(const T& t)
{
if (t != DesiredSize) std::cerr << "t : " << t << " != DesiredSize : " << DesiredSize << std::endl;
assert(t == DesiredSize && "Size of dynamic Vector or Matrix does not match");
}
};
template<int DesiredR, int DesiredC>
template <int DesiredR, int DesiredC>
struct MatrixSizeCheck
{
/** \brief Assert matrix size dynamically or statically
*
* @param t the variable for which we wish to assert the size.
*
* Usage: to assert that t is size (Rows x Cols)
*
* MatrixSizeCheck<Rows, Cols>::check(t);
*/
template<typename T>
static void check(const Eigen::MatrixBase<T>& t)
{
StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows());
StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols());
}
/** \brief Assert matrix size dynamically or statically
*
* @param t the variable for which we wish to assert the size.
*
* Usage: to assert that t is size (Rows x Cols)
*
* MatrixSizeCheck<Rows, Cols>::check(t);
*/
template <typename T>
static void check(const Eigen::MatrixBase<T>& t)
{
StaticDimCheck<Eigen::MatrixBase<T>::RowsAtCompileTime, DesiredR>(t.rows());
StaticDimCheck<Eigen::MatrixBase<T>::ColsAtCompileTime, DesiredC>(t.cols());
}
};
template <int Dim>
......@@ -106,5 +104,3 @@ template <int Dim>
using RowVectorSizeCheck = MatrixSizeCheck<1, Dim>;
} /* namespace Eigen */
#endif /* _WOLF_EIGEN_ASSERT_H_ */
include/core/utils/folder_registry.h 0 → 100644
Edit View file @ 44e08b4a
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/utils/unused.h"
// std
#include <string>
#include <map>
#include <vector>
#include <iostream>
#include <iomanip>
namespace wolf
{
class FolderRegistry
{
private:
std::map<std::string, std::string> folders_;
public:
static bool registerFolder(const std::string& _plugin, const std::string _folder);
static bool unregisterFolder(const std::string& _plugin);
static bool isFolderRegistered(const std::string& _plugin);
static void printAddress();
static std::vector<std::string> getRegisteredFolders();
static void appendRegisteredFolders(std::vector<std::string>& _folders);
// Singleton ---------------------------------------------------
// This class is a singleton. The code below guarantees this.
// See: http://stackoverflow.com/questions/1008019/c-singleton-design-pattern
private:
static FolderRegistry& get();
public:
FolderRegistry(const FolderRegistry&) = delete;
void operator=(FolderRegistry const&) = delete;
private:
FolderRegistry();
~FolderRegistry();
};
inline FolderRegistry::FolderRegistry()
{
// std::cout << " FolderRegistry constructor " << std::endl;
}
inline FolderRegistry::~FolderRegistry()
{
// std::cout << " FolderRegistry destructor " << std::endl;}
}
inline bool FolderRegistry::registerFolder(const std::string& _plugin, const std::string _folder)
{
// Unregister plugin-folder if registered with different folder
if (get().isFolderRegistered(_plugin) and get().folders_.at(_plugin) != _folder)
{
std::cout << std::setw(31) << std::left << "FolderRegistry"
<< " XXX unregister old '" << _plugin << "' folder: " << get().folders_.at(_plugin) << std::endl;
get().unregisterFolder(_plugin);
}
// register plugin-folder if not registered
if (not get().isFolderRegistered(_plugin))
{
get().folders_.insert(std::pair<std::string, std::string>(_plugin, _folder)).second;
std::cout << std::setw(31) << std::left << "FolderRegistry"
<< " <-- registered '" << _plugin << "' folder: " << _folder << std::endl;
return true;
}
return false;
}
inline bool FolderRegistry::unregisterFolder(const std::string& _plugin)
{
if (get().isFolderRegistered(_plugin))
return get().folders_.erase(_plugin) == 1;
else
std::cout << std::setw(31) << std::left << "FolderRegistry"
<< " ERROR trying to unregister " << _plugin << ": Not registered." << std::endl;
return false;
}
inline bool FolderRegistry::isFolderRegistered(const std::string& _plugin)
{
return get().folders_.count(_plugin);
}
inline FolderRegistry& FolderRegistry::get()
{
static FolderRegistry instance_;
return instance_;
}
inline void FolderRegistry::printAddress()
{
std::cout << "FolderRegistry address: " << &get() << std::endl;
}
inline std::vector<std::string> FolderRegistry::getRegisteredFolders()
{
std::vector<std::string> vec;
for (auto pair : get().folders_) vec.push_back(pair.second);
return vec;
}
inline void FolderRegistry::appendRegisteredFolders(std::vector<std::string>& _folders)
{
for (auto pair : get().folders_) _folders.push_back(pair.second);
}
} // namespace wolf
#define WOLF_REGISTER_FOLDER(plugin, folder) \
namespace \
{ \
const bool WOLF_UNUSED folder##Registered = FolderRegistry::registerFolder(plugin, folder); \
}
include/core/utils/graph_search.h
Edit View file @ 44e08b4a
//--------LICENSE_START--------
//
// Copyright (C) 2020,2021,2022,2023,2024 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu)
// WOLF - Copyright (C) 2020-2025
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// at your option) any later version.
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
//--------LICENSE_END--------
#ifndef GRAPH_SEARCH_H
#define GRAPH_SEARCH_H
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
#include "core/common/wolf.h"
#include "core/frame/frame_base.h"
......@@ -30,36 +26,30 @@
namespace wolf
{
class GraphSearch
{
private:
std::map<FrameBasePtr, std::pair<FactorBasePtr,FrameBasePtr>> parents_;
private:
std::map<NodeStateBlocksPtr, std::pair<FactorBasePtr, NodeStateBlocksPtr>> parents_;
public:
public:
GraphSearch();
GraphSearch();
~GraphSearch();
~GraphSearch();
FactorBasePtrList computeShortestPath(NodeStateBlocksPtr node1,
NodeStateBlocksPtr node2,
const unsigned int max_graph_dist = 0);
FactorBasePtrList computeShortestPath(FrameBasePtr frm1,
FrameBasePtr frm2,
const unsigned int max_graph_dist = 0);
std::set<NodeStateBlocksPtr> getNeighborFrames(const std::set<NodeStateBlocksPtr>& nodes);
std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
static FactorBasePtrList shortestPath(FrameBasePtr frm1,
FrameBasePtr frm2,
const unsigned int max_graph_dist = 0)
{
GraphSearch graph_search;
return graph_search.computeShortestPath(frm1, frm2, max_graph_dist);
}
static FactorBasePtrList shortestPath(NodeStateBlocksPtr node1,
NodeStateBlocksPtr node2,
const unsigned int max_graph_dist = 0)
{
GraphSearch graph_search;
return graph_search.computeShortestPath(node1, node2, max_graph_dist);
}
};
} // namespace wolf
#endif