Skip to content
Snippets Groups Projects
Commit b659f2c9 authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

[skip ci] adapting to yaml node and pragma once

parent d9c2705d
No related branches found
No related tags found
1 merge request!448Draft: Resolve "Implementation of new nodes creation"
Pipeline #12020 skipped
Showing
with 114 additions and 271 deletions
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file sensor_pose.h
*
* Created on: Feb 18, 2020
* \author: mfourmy
*/
#ifndef SRC_SENSOR_POSE_H_
#define SRC_SENSOR_POSE_H_
#pragma once
//wolf includes
#include "core/sensor/sensor_base.h"
......@@ -66,8 +58,7 @@ class SensorPose : public SensorBase
ParamsSensorPosePtr params_pose_;
public:
SensorPose(const std::string& _unique_name,
const SizeEigen& _dim,
SensorPose(const SizeEigen& _dim,
ParamsSensorPosePtr _params,
const Priors& _priors);
......@@ -92,6 +83,4 @@ inline double SensorPose::getStdO() const
return params_pose_->std_o;
}
} /* namespace wolf */
#endif /* SRC_SENSOR_POSE_H_ */
} /* namespace wolf */
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file factory_solver.h
*
* Created on: Dec 17, 2018
* \author: jcasals
*/
#ifndef FACTORY_SOLVER_H_
#define FACTORY_SOLVER_H_
#pragma once
namespace wolf
{
......@@ -39,6 +31,8 @@ struct ParamsSensorBase;
#include "core/common/factory.h"
#include "core/solver/solver_manager.h"
#include "yaml-cpp/yaml.h"
namespace wolf
{
......@@ -48,7 +42,7 @@ namespace wolf
typedef Factory<SolverManager,
const ProblemPtr&,
const ParamsServer&> FactorySolver;
const YAML::Node&> FactorySolver;
template<>
inline std::string FactorySolver::getClass() const
......@@ -60,6 +54,4 @@ inline std::string FactorySolver::getClass() const
namespace{ const bool WOLF_UNUSED SolverType##Registered = \
wolf::FactorySolver::registerCreator(#SolverType, SolverType::create); } \
} /* namespace wolf */
#endif /* SENSOR_FACTORY_H_ */
} /* namespace wolf */
\ No newline at end of file
......@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef _WOLF_SOLVER_MANAGER_H_
#define _WOLF_SOLVER_MANAGER_H_
#pragma once
//wolf includes
#include "core/common/wolf.h"
......@@ -45,19 +44,13 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsSolver)
* SolverManagerClass(const ProblemPtr& wolf_problem,
* const ParamsSolverClassPtr _params);
*/
#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
static SolverManagerPtr create(const ProblemPtr& _problem, \
const ParamsServer& _server) \
{ \
auto params = std::make_shared<ParamsSolverClass>(#SolverClass, _server); \
return std::make_shared<SolverClass>(_problem, params); \
} \
static SolverManagerPtr create(const ProblemPtr& _problem, \
const ParamsSolverPtr _params) \
{ \
auto params = std::static_pointer_cast<ParamsSolverClass>(_params); \
return std::make_shared<SolverClass>(_problem, params); \
} \
#define WOLF_SOLVER_CREATE(SolverClass, ParamsSolverClass) \
static SolverManagerPtr create(const ProblemPtr& _problem, \
const YAML::Node& _input_node) \
{ \
auto params = std::make_shared<ParamsSolverClass>(_input_node); \
return std::make_shared<SolverClass>(_problem, params); \
} \
struct ParamsSolver;
......@@ -237,7 +230,6 @@ class SolverManager
// Params (here becaure it needs of declaration of SolverManager::ReportVerbosity)
struct ParamsSolver: public ParamsBase
{
std::string prefix = "solver/";
double period = 0.0;
SolverManager::ReportVerbosity verbose = SolverManager::ReportVerbosity::QUIET;
bool compute_cov = false;
......@@ -245,16 +237,16 @@ struct ParamsSolver: public ParamsBase
double cov_period = 1.0;
ParamsSolver() = default;
ParamsSolver(std::string _unique_name, const ParamsServer& _server):
ParamsBase(_unique_name, _server)
ParamsSolver(const YAML::Node& _input_node):
ParamsBase(_input_node)
{
period = _server.getParam<double>(prefix + "period");
verbose = (SolverManager::ReportVerbosity)_server.getParam<int>(prefix + "verbose");
compute_cov = _server.getParam<bool>(prefix + "compute_cov");
period = _input_node["period"].as<double>();
verbose = (SolverManager::ReportVerbosity)_input_node["verbose"].as<int>();
compute_cov = _input_node["compute_cov"].as<bool>();
if (compute_cov)
{
cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_server.getParam<int>(prefix + "cov_enum");
cov_period = _server.getParam<double>(prefix + "cov_period");
cov_enum = (SolverManager::CovarianceBlocksToBeComputed)_input_node["cov_enum"].as<int>();
cov_period = _input_node["cov_period"].as<double>();
}
}
std::string print() const override
......@@ -269,6 +261,4 @@ struct ParamsSolver: public ParamsBase
~ParamsSolver() override = default;
};
} // namespace wolf
#endif /* _WOLF_SOLVER_MANAGER_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* ccolamd_ordering.h
*
* Created on: Jun 12, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
#define TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_
#pragma once
//std includes
#include <iostream>
......@@ -96,6 +88,4 @@ class CCOLAMDOrdering
return info;
}
};
}
#endif /* TRUNK_SRC_WOLF_SOLVER_CCOLAMD_ORDERING_H_ */
}
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* cost_function_base.h
*
* Created on: Jun 25, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_
#pragma once
#include "core/common/wolf.h"
#include <Eigen/StdVector>
......@@ -113,6 +105,4 @@ class CostFunctionBase
for (unsigned int i = 0; i<n_blocks_; i++)
jacobians.at(i) = (*jacobians_.at(i));
}
};
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_BASE_H_ */
};
\ No newline at end of file
......@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_
#pragma once
//wolf includes
#include "core/common/wolf.h"
......@@ -525,6 +524,4 @@ class CostFunctionSparse<FactorT,
}
};
} //namespace wolf
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_H_ */
} //namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* cost_function_sparse.h
*
* Created on: Jun 25, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
#define TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_
#pragma once
//wolf includes
#include "core/common/wolf.h"
......@@ -341,6 +333,4 @@ const unsigned int MEASUREMENT_SIZE,
jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i);
}
} // wolf namespace
#endif /* TRUNK_SRC_SOLVER_COST_FUNCTION_SPARSE_BASE_H_ */
} // wolf namespace
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* qr_solver.h
*
* Created on: Jul 2, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_QR_SOLVER_H_
#define TRUNK_SRC_SOLVER_QR_SOLVER_H_
#pragma once
//std includes
#include <core/factor/factor_odom_2d.h>
......@@ -610,6 +602,4 @@ class SolverQR
}
};
} // namespace wolf
#endif /* TRUNK_SRC_SOLVER_QR_SOLVER_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* solver_QR.h
*
* Created on: Jun 22, 2015
* Author: jvallve
*/
#ifndef TRUNK_SRC_SOLVER_SOLVER_QR_H_
#define TRUNK_SRC_SOLVER_SOLVER_QR_H_
#pragma once
using namespace Eigen;
......@@ -48,6 +40,4 @@ class SolverQR
VectorXi nodes_ordering_factors;
private:
};
#endif /* TRUNK_SRC_SOLVER_SOLVER_QR_H_ */
};
\ No newline at end of file
......@@ -19,8 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef CERES_MANAGER_H_
#define CERES_MANAGER_H_
#pragma once
//wolf includes
#include "core/factor/factor_GPS_2d.h"
......@@ -64,6 +63,4 @@ class SolverManager
void updateStateUnitStatus(StateBlockPtr _st_ptr);
ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
};
#endif
};
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* sparse_utils.h
*
* Created on: Jul 2, 2015
* Author: jvallve
*/
#ifndef SPARSE_UTILS_H_
#define SPARSE_UTILS_H_
#pragma once
// eigen includes
#include <eigen3/Eigen/Sparse>
......@@ -63,5 +55,4 @@ void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& origina
original.makeCompressed();
}
}
#endif /* SPARSE_UTILS_H_ */
}
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* \file factory_state_block.h
*
* Created on: Apr 27, 2020
* \author: jsola
*/
#ifndef STATE_BLOCK_FACTORY_STATE_BLOCK_H_
#define STATE_BLOCK_FACTORY_STATE_BLOCK_H_
#pragma once
#include "core/common/factory.h"
#include "core/state_block/state_block.h"
......@@ -149,18 +141,12 @@ inline StateBlockPtr FactoryStateBlock::create(const string& _type, const Eigen:
return (creator_callback_it->second)(_state, _fixed);
}
#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); } \
#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
}
#define WOLF_REGISTER_STATEBLOCK(StateBlockType) \
namespace{ const bool WOLF_UNUSED StateBlockType##Registered = \
FactoryStateBlock::registerCreator(#StateBlockType, StateBlockType::create); } \
#define WOLF_REGISTER_STATEBLOCK_WITH_KEY(Key, StateBlockType) \
namespace{ const bool WOLF_UNUSED StateBlockType##RegisteredWith##Key = \
FactoryStateBlock::registerCreator(#Key, StateBlockType::create); } \
#endif /* STATE_BLOCK_FACTORY_STATE_BLOCK_H_ */
}
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file has_state_blocks.h
*
* Created on: Aug 27, 2019
* \author: jsola
*/
#ifndef STATE_BLOCK_HAS_STATE_BLOCKS_H_
#define STATE_BLOCK_HAS_STATE_BLOCKS_H_
#pragma once
#include "core/common/wolf.h"
#include "core/state_block/state_composite.h"
......@@ -444,6 +436,4 @@ inline unsigned int HasStateBlocks::getLocalSize(const StateStructure& _structur
return size;
}
} // namespace wolf
#endif /* STATE_BLOCK_HAS_STATE_BLOCKS_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file local_parametrization_angle.h
*
* Created on: Apr 4, 2017
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_ANGLE_H_
#define LOCAL_PARAMETRIZATION_ANGLE_H_
#pragma once
#include "core/state_block/local_parametrization_base.h"
#include "core/math/rotations.h"
......@@ -92,6 +84,4 @@ inline bool LocalParametrizationAngle::isValid(const Eigen::VectorXd& _state, do
//Eigen::Map not Eigen::VectorXd
return true;
}
} /* namespace wolf */
#endif /* LOCAL_PARAMETRIZATION_ANGLE_H_ */
} /* namespace wolf */
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_base.h
*
* Created on: Feb 17, 2016
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_BASE_H_
#define LOCAL_PARAMETRIZATION_BASE_H_
#pragma once
#include "core/common/wolf.h"
......@@ -57,6 +49,4 @@ class LocalParametrizationBase{
unsigned int getGlobalSize() const;
};
} // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_BASE_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_homogeneous.h
*
* Created on: 24/02/2016
* Author: jsola
*/
#ifndef LOCALPARAMETRIZATIONHOMOGENEOUS_H_
#define LOCALPARAMETRIZATIONHOMOGENEOUS_H_
#pragma once
#include "core/state_block/local_parametrization_base.h"
......@@ -71,6 +63,4 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
bool isValid(const Eigen::VectorXd& state, double tolerance) override;
};
} // namespace wolf
#endif /* LOCALPARAMETRIZATIONHOMOGENEOUS_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/*
* \file local_parametrization_quaternion.h
*
* Created on: Feb 18, 2016
* \author: jsola
*/
#ifndef LOCAL_PARAMETRIZATION_QUATERNION_H_
#define LOCAL_PARAMETRIZATION_QUATERNION_H_
#pragma once
#include "core/state_block/local_parametrization_base.h"
......@@ -105,6 +97,4 @@ public:
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
typedef LocalParametrizationQuaternion<DQ_LOCAL> LocalParametrizationQuaternionLocal;
} // namespace wolf
#endif /* LOCAL_PARAMETRIZATION_QUATERNION_H_ */
} // namespace wolf
\ No newline at end of file
......@@ -19,55 +19,69 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef PRIOR_H_
#define PRIOR_H_
#pragma once
#include <string>
#include <unordered_map>
#include <eigen3/Eigen/Dense>
#include "yaml-cpp/yaml.h"
namespace wolf
{
class ParamsServer;
class Prior;
typedef std::unordered_map<char, Prior> Priors;
class Prior
{
private:
std::string type_; // State type
Eigen::VectorXd state_; // state values
std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
bool dynamic_; // State dynamic
Eigen::VectorXd drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
std::string type_; // State type
Eigen::VectorXd state_; // state values
std::string mode_; // Prior mode. Can be 'initial_guess', 'fix' or 'factor'
Eigen::VectorXd noise_std_; // factor noise std, sqrt of the diagonal of the covariance mtrix (ONLY IF mode == 'factor')
bool dynamic_; // State dynamic
Eigen::VectorXd drift_std_; // OPTIONAL std of the state drift [in units/sqrt(s)] diagonal elements (ONLY IF if dynamic)
public:
Prior() = default;
Prior(const std::string& _type,
const Eigen::VectorXd& _state,
const std::string& _mode = "fix",
const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
bool _dynamic = false,
const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
Prior(const std::string& _prefix, const std::string& _type, const ParamsServer& _server);
virtual ~Prior() = default;
const std::string& getType() const;
const std::string& getMode() const;
const Eigen::VectorXd& getState() const;
const Eigen::VectorXd& getNoiseStd() const;
bool isDynamic() const;
bool isFixed() const;
bool isInitialGuess() const;
bool isFactor() const;
const Eigen::VectorXd& getDriftStd() const;
void check() const;
virtual std::string print() const final;
Prior() = default;
Prior(const std::string& _type,
const Eigen::VectorXd& _state,
const std::string& _mode = "fix",
const Eigen::VectorXd& _noise_std = Eigen::VectorXd(0),
bool _dynamic = false,
const Eigen::VectorXd& _drift_std = Eigen::VectorXd(0));
Prior(const YAML::Node& prior_node);
virtual ~Prior() = default;
const std::string& getType() const;
const std::string& getMode() const;
const Eigen::VectorXd& getState() const;
const Eigen::VectorXd& getNoiseStd() const;
bool isDynamic() const;
bool isFixed() const;
bool isInitialGuess() const;
bool isFactor() const;
const Eigen::VectorXd& getDriftStd() const;
void check() const;
virtual std::string print() const final;
};
typedef std::unordered_map<char, Prior> PriorMap;
class Priors : public PriorMap
{
public:
using PriorMap::PriorMap;
Priors(const YAML::Node& priors_node)
{
for (auto prior_pair : priors_node)
{
this->emplace(prior_pair.first.as<char>(), Prior(prior_pair.second));
}
}
virtual ~Priors() = default;
};
inline const std::string& Prior::getType() const { return type_; }
......@@ -87,5 +101,5 @@ inline bool Prior::isInitialGuess() const { return mode_ == "initial_guess"; }
inline bool Prior::isFactor() const { return mode_ == "factor"; }
inline const Eigen::VectorXd& Prior::getDriftStd() const { return drift_std_; }
} // namespace wolf
#endif
} // namespace wolf
\ No newline at end of file
......@@ -19,15 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
/**
* \file state_angle.h
*
* Created on: Apr 4, 2017
* \author: jsola
*/
#ifndef STATE_ANGLE_H_
#define STATE_ANGLE_H_
#pragma once
#include "core/state_block/state_block.h"
#include "core/state_block/local_parametrization_angle.h"
......@@ -71,6 +63,4 @@ inline void StateAngle::transform(const VectorComposite& _transformation)
if (isTransformable()) setState(_transformation.at('O') + getState()); // 2D rotation is a sum of angles
}
} /* namespace wolf */
#endif /* STATE_ANGLE_H_ */
} /* namespace wolf */
\ No newline at end of file
......@@ -19,9 +19,7 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
//
//--------LICENSE_END--------
#ifndef STATE_BLOCK_H_
#define STATE_BLOCK_H_
#pragma once
// Fwd references
namespace wolf{
......@@ -354,6 +352,5 @@ inline bool StateBlock::isValid(double tolerance)
{
return local_param_ptr_ ? local_param_ptr_->isValid(state_, tolerance) : true;
}
}// namespace wolf
#endif
}// namespace wolf
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment