Skip to content
Snippets Groups Projects

Draft: Resolve "SensorBase: extrinsic and intrinsic initialization and prior"

Files
6
@@ -65,7 +65,13 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
} \
struct ParamsStateBlock
{
Eigen::VectorXd state;
std::string prior_mode;
Eigen::VectorXd sigma;
bool dynamic;
};
/** \brief base struct for intrinsic sensor parameters
*
@@ -74,8 +80,103 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex
struct ParamsSensorBase: public ParamsBase
{
std::string prefix = "sensor/";
int dim;
std::unordered_map<char, ParamsStateBlock> state_blocks;
ParamsSensorBase() = default;
ParamsSensorBase(std::string _unique_name, const ParamsServer& _server) :
ParamsBase(_unique_name, _server)
{
dim = _server.getParam<int>("problem/dimension");
if (dim != 2 and dim != 3)
{
WOLF_ERROR("ParamsSensorBase: parameter 'problem/dimension' should be 2 or 3. Value provided: ", dim);
throw std::runtime_error("ParamsSensorBase wrong 'problem/dimension'");
}
// P
state_blocks.emplace('P',ParamsStateBlock());
if (dim == 2)
{
state_blocks['P'].state = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/state");
state_blocks['P'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/P/prior_mode");
state_blocks['P'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/P/is_dynamic");
if (state_blocks['P'].prior_mode == "factor")
state_blocks['P'].sigma = _server.getParam<Eigen::Vector2d> (prefix + _unique_name + "/P/sigma");
}
else if (dim == 3)
{
state_blocks['P'].state = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/state");
state_blocks['P'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/P/prior_mode");
state_blocks['P'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/P/is_dynamic");
if (state_blocks['P'].prior_mode == "factor")
state_blocks['P'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/P/sigma");
}
assert((state_blocks['P'].prior_mode == "initial_guess" || state_blocks['P'].prior_mode == "fix" || state_blocks['P'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
// O
state_blocks.emplace('O',ParamsStateBlock());
if (dim == 2)
{
state_blocks['O'].state = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/state");
state_blocks['O'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/O/prior_mode");
state_blocks['O'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/O/is_dynamic");
if (state_blocks['O'].prior_mode == "factor")
state_blocks['O'].sigma = _server.getParam<Eigen::Vector1d> (prefix + _unique_name + "/O/sigma");
}
else if (dim == 3)
{
state_blocks['O'].state = _server.getParam<Eigen::Vector4d> (prefix + _unique_name + "/O/state");
state_blocks['O'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/O/prior_mode");
state_blocks['O'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/O/is_dynamic");
if (state_blocks['O'].prior_mode == "factor")
state_blocks['O'].sigma = _server.getParam<Eigen::Vector3d> (prefix + _unique_name + "/O/sigma");
}
assert((state_blocks['O'].prior_mode == "initial_guess" || state_blocks['O'].prior_mode == "fix" || state_blocks['O'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
// I
try
{
state_blocks.emplace('I',ParamsStateBlock());
state_blocks['I'].state = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/state");
state_blocks['I'].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/I/prior_mode");
state_blocks['I'].dynamic = _server.getParam<bool> (prefix + _unique_name + "/I/is_dynamic");
if (state_blocks['I'].prior_mode == "factor")
state_blocks['I'].sigma = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/I/sigma");
assert((state_blocks['I'].prior_mode == "initial_guess" || state_blocks['I'].prior_mode == "fix" || state_blocks['I'].prior_mode == "factor") && "wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
}
catch(...)
{
WOLF_INFO("ParamsSensorBase: Prior for the intrinsics not provided for sensor ", _unique_name);
}
}
~ParamsSensorBase() override = default;
using ParamsBase::ParamsBase;
void getStateBlockPrior(const ParamsServer& _server, const std::string& _unique_name, const char _key, bool _mandatory = false)
{
try
{
state_blocks.emplace(_key,ParamsStateBlock());
state_blocks[_key].state = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/" + std::to_string(_key) + "/state");
state_blocks[_key].prior_mode = _server.getParam<std::string> (prefix + _unique_name + "/" + std::to_string(_key) + "/prior_mode");
state_blocks[_key].dynamic = _server.getParam<bool> (prefix + _unique_name + "/" + std::to_string(_key) + "/is_dynamic");
if (state_blocks[_key].prior_mode == "factor")
state_blocks[_key].sigma = _server.getParam<Eigen::VectorXd> (prefix + _unique_name + "/" + std::to_string(_key) + "/sigma");
assert((state_blocks[_key].prior_mode == "initial_guess" or
state_blocks[_key].prior_mode == "fix" or
state_blocks[_key].prior_mode == "factor") and
"wrong prior_mode value, it should be: 'initial_guess', 'fix' or 'factor'");
}
catch(...)
{
if (_mandatory)
throw std::runtime_error("Prior for state block " + std::to_string(_key) + " not found");
}
}
std::string print() const override
{
return "";
@@ -108,6 +209,17 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
void setProblem(ProblemPtr _problem) override final;
public:
/** \brief Constructor with params struct
*
* Constructor with parameter struct
* \param _tp Type of the sensor (types defined at wolf.h)
* \param _params ParamsSensorBasePtr pointer to params struct
*
**/
SensorBase(const std::string& _type,
ParamsSensorBasePtr _params,
const int& _intrinsics_size);
/** \brief Constructor with noise size
*
* Constructor with parameter vector
Loading