From 9b799315e4da9098203485f626be04c2aa8557ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Wed, 27 Jul 2022 15:50:35 +0200 Subject: [PATCH] finally working everything --- demos/hello_wolf/hello_wolf.cpp | 41 +++--- demos/hello_wolf/hello_wolf_autoconf.cpp | 19 +-- .../schema/ProcessorRangeBearing.schema | 1 + .../schema/SensorRangeBearing.schema | 19 +++ demos/hello_wolf/sensor_range_bearing.cpp | 6 +- demos/hello_wolf/sensor_range_bearing.h | 3 +- demos/hello_wolf/yaml/hello_wolf_config.yaml | 105 +++++++-------- demos/hello_wolf/yaml/sensor_odom_2d.yaml | 3 + include/core/capture/capture_base.h | 2 + include/core/problem/problem.h | 31 ++--- include/core/sensor/sensor_base.h | 4 +- include/core/state_block/has_state_blocks.h | 3 +- schema/none.schema | 6 + schema/processor/ProcessorOdom2d.schema | 7 + src/capture/capture_base.cpp | 5 + src/frame/frame_base.cpp | 2 +- src/landmark/landmark_base.cpp | 2 +- src/problem/problem.cpp | 126 +++++++----------- src/sensor/sensor_base.cpp | 5 + test/gtest_odom_2d.cpp | 12 +- test/gtest_problem.cpp | 2 +- 21 files changed, 213 insertions(+), 191 deletions(-) create mode 100644 demos/hello_wolf/schema/ProcessorRangeBearing.schema create mode 100644 demos/hello_wolf/schema/SensorRangeBearing.schema create mode 100644 schema/none.schema create mode 100644 schema/processor/ProcessorOdom2d.schema diff --git a/demos/hello_wolf/hello_wolf.cpp b/demos/hello_wolf/hello_wolf.cpp index b64421fd8..6eb046afc 100644 --- a/demos/hello_wolf/hello_wolf.cpp +++ b/demos/hello_wolf/hello_wolf.cpp @@ -132,7 +132,9 @@ int main() ParamsSensorOdomPtr intrinsics_odo = std::make_shared<ParamsSensorOdom>(); SpecSensorComposite priors_odo = {{'P',SpecStateSensor("StatePoint2d", Vector2d::Zero())}, {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}}; - SensorBasePtr sensor_odo = problem->installSensor("SensorOdom", "sensor odo", intrinsics_odo, priors_odo); + SensorBasePtr sensor_odo = SensorBase::emplace<SensorOdom2d>(problem->getHardware(), + intrinsics_odo, + priors_odo); // processor odometer 2d ParamsProcessorOdom2dPtr params_odo = std::make_shared<ParamsProcessorOdom2d>(); @@ -143,29 +145,35 @@ int main() params_odo->angle_turned = 999; params_odo->cov_det = 999; params_odo->unmeasured_perturbation_std = 0.001; - ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2d", "processor odo", sensor_odo, params_odo); + ProcessorBasePtr processor = ProcessorBase::emplace<ProcessorOdom2d>(sensor_odo, params_odo); // sensor Range and Bearing ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>(); - SpecSensorComposite priors_rb = {{'P',SpecStateSensor("P", Vector2d(1,1))}, - {'O',SpecStateSensor("O", Vector1d::Zero())}}; + SpecSensorComposite priors_rb = {{'P',SpecStateSensor("StatePoint2d", Vector2d(1,1))}, + {'O',SpecStateSensor("StateAngle", Vector1d::Zero())}}; intrinsics_rb->noise_range_metres_std = 0.1; intrinsics_rb->noise_bearing_degrees_std = 1.0; - SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", intrinsics_rb, priors_rb); + SensorBasePtr sensor_rb = SensorBase::emplace<SensorRangeBearing>(problem->getHardware(), + intrinsics_rb, + priors_rb); // processor Range and Bearing ParamsProcessorRangeBearingPtr params_rb = std::make_shared<ParamsProcessorRangeBearing>(); params_rb->voting_active = false; params_rb->time_tolerance = 0.01; - ProcessorBasePtr processor_rb = problem->installProcessor("ProcessorRangeBearing", "processor RB", sensor_rb, params_rb); + ProcessorBasePtr processor_rb = ProcessorBase::emplace<ProcessorRangeBearing>(sensor_rb, params_rb); // initialize - TimeStamp t(0.0); // t : 0.0 - // Vector3d x(0,0,0); - VectorComposite x(Vector3d(0,0,0), "PO", {2,1}); - // Matrix3d P = Matrix3d::Identity() * 0.1; - VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); - FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0) + TimeStamp t(0.0); // t : 0.0 + SpecComposite prior; + prior.emplace('P',SpecState("StatePoint2d", Vector2d::Zero(), "factor", Vector2d::Constant(sqrt(0.1)))); + prior.emplace('O',SpecState("StateAngle", Vector1d::Zero(), "factor", Vector1d::Constant(sqrt(0.1)))); + FrameBasePtr KF1 = problem->setPrior(prior, t); // KF1 : (0,0,0) + // // Vector3d x(0,0,0); + // VectorComposite x(Vector3d(0,0,0), "PO", {2,1}); + // // Matrix3d P = Matrix3d::Identity() * 0.1; + // VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1}); + // FrameBasePtr KF1 = problem->setPriorFactor(x, P, t); // KF1 : (0,0,0) std::static_pointer_cast<ProcessorMotion>(processor)->setOrigin(KF1); // SELF CALIBRATION =================================================== @@ -256,17 +264,20 @@ int main() // GET COVARIANCES of all states WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======") - ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + bool succeed = ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS); + WOLF_ERROR_COND(not succeed, "Covariances could not be computed."); for (auto& kf_pair : problem->getTrajectory()->getFrameMap()) { Eigen::MatrixXd cov; - kf_pair.second->getCovariance(cov); + succeed = kf_pair.second->getCovariance("PO", cov); + WOLF_ERROR_COND(not succeed, "Covariance matrix for frame ", kf_pair.second->id(), " could not be recovered."); WOLF_TRACE("KF", kf_pair.second->id(), "_cov = \n", cov); } for (auto& lmk : problem->getMap()->getLandmarkList()) { Eigen::MatrixXd cov; - lmk->getCovariance(cov); + succeed = lmk->getCovariance("P", cov); + WOLF_ERROR_COND(not succeed, "Covariance matrix for landmark ", lmk->id(), " could not be recovered."); WOLF_TRACE("L", lmk->id(), "_cov = \n", cov); } std::cout << std::endl; diff --git a/demos/hello_wolf/hello_wolf_autoconf.cpp b/demos/hello_wolf/hello_wolf_autoconf.cpp index 13e92f792..b8cc00e1e 100644 --- a/demos/hello_wolf/hello_wolf_autoconf.cpp +++ b/demos/hello_wolf/hello_wolf_autoconf.cpp @@ -33,7 +33,6 @@ #include "core/common/wolf.h" #include "core/capture/capture_odom_2d.h" #include "core/processor/processor_motion.h" -#include "core/yaml/parser_yaml.h" #include "capture_range_bearing.h" @@ -123,16 +122,20 @@ int main() std::string wolf_path = _WOLF_ROOT_DIR; // parse file into params server: each param will be retrievable from this params server: - ParserYaml parser = ParserYaml(wolf_path + "/" + config_file); - ParamsServer server = ParamsServer(parser.getParams()); + yaml_schema_cpp::YamlServer server({wolf_path}, wolf_path + "/" + config_file); + if (not server.applySchema("Problem2d")) + { + WOLF_ERROR(server.getLog()); + throw std::runtime_error("Couldn't load and validate the yaml file " + config_file); + } // Wolf problem: automatically build the left branch of the wolf tree from the contents of the params server: - ProblemPtr problem = Problem::autoSetup(server); - + ProblemPtr problem = Problem::autoSetup(server.getNode()); + // Print problem to see its status before processing any sensor data problem->print(4,0,1,0); // Solver. Configure a Ceres solver - SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server); + SolverManagerPtr ceres = FactorySolver::create("SolverCeres", problem, server.getNode()["solver"]); // recover sensor pointers and other stuff for later use (access by sensor name) SensorBasePtr sensor_odo = problem->findSensor("sen odom"); @@ -243,13 +246,13 @@ int main() for (auto& kf_pair : problem->getTrajectory()->getFrameMap()) { Eigen::MatrixXd cov; - kf_pair.second->getCovariance(cov); + kf_pair.second->getCovariance("PO", cov); WOLF_INFO("KF", kf_pair.second->id(), "_cov = \n", cov); } for (auto& lmk : problem->getMap()->getLandmarkList()) { Eigen::MatrixXd cov; - lmk->getCovariance(cov); + lmk->getCovariance("P", cov); WOLF_INFO("L", lmk->id(), "_cov = \n", cov); } std::cout << std::endl; diff --git a/demos/hello_wolf/schema/ProcessorRangeBearing.schema b/demos/hello_wolf/schema/ProcessorRangeBearing.schema new file mode 100644 index 000000000..3c97f7469 --- /dev/null +++ b/demos/hello_wolf/schema/ProcessorRangeBearing.schema @@ -0,0 +1 @@ +follow: ProcessorBase.schema \ No newline at end of file diff --git a/demos/hello_wolf/schema/SensorRangeBearing.schema b/demos/hello_wolf/schema/SensorRangeBearing.schema new file mode 100644 index 000000000..bc40eef77 --- /dev/null +++ b/demos/hello_wolf/schema/SensorRangeBearing.schema @@ -0,0 +1,19 @@ +follow: SensorBase.schema + +noise_range_metres_std: + mandatory: true + type: double + yaml_type: scalar + doc: Standard deviation of the noise of the range measurements (meters) + +noise_bearing_degrees_std: + mandatory: true + type: double + yaml_type: scalar + doc: Standard deviation of the noise of the bearing measurements (degrees) + +states: + P: + follow: SpecStateSensorP2d.schema + O: + follow: SpecStateSensorO2d.schema \ No newline at end of file diff --git a/demos/hello_wolf/sensor_range_bearing.cpp b/demos/hello_wolf/sensor_range_bearing.cpp index 0738c4418..b7d62c95d 100644 --- a/demos/hello_wolf/sensor_range_bearing.cpp +++ b/demos/hello_wolf/sensor_range_bearing.cpp @@ -25,13 +25,11 @@ namespace wolf{ WOLF_PTR_TYPEDEFS(SensorRangeBearing); -SensorRangeBearing::SensorRangeBearing(const SizeEigen& _dim, - ParamsSensorRangeBearingPtr _params, +SensorRangeBearing::SensorRangeBearing(ParamsSensorRangeBearingPtr _params, const SpecSensorComposite& _priors) : - SensorBase("SensorRangeBearing", _dim, _params, _priors), + SensorBase("SensorRangeBearing", 2, _params, _priors("PO")), params_rb_(_params) { - assert(_dim == 2 && "SensorRangeBearing only for 2D"); } SensorRangeBearing::~SensorRangeBearing() diff --git a/demos/hello_wolf/sensor_range_bearing.h b/demos/hello_wolf/sensor_range_bearing.h index 75d6e7731..69e0ea53c 100644 --- a/demos/hello_wolf/sensor_range_bearing.h +++ b/demos/hello_wolf/sensor_range_bearing.h @@ -56,8 +56,7 @@ class SensorRangeBearing : public SensorBase ParamsSensorRangeBearingPtr params_rb_; public: - SensorRangeBearing(const SizeEigen& _dim, - ParamsSensorRangeBearingPtr _params, + SensorRangeBearing(ParamsSensorRangeBearingPtr _params, const SpecSensorComposite& _priors); WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing); diff --git a/demos/hello_wolf/yaml/hello_wolf_config.yaml b/demos/hello_wolf/yaml/hello_wolf_config.yaml index 33889188f..267616859 100644 --- a/demos/hello_wolf/yaml/hello_wolf_config.yaml +++ b/demos/hello_wolf/yaml/hello_wolf_config.yaml @@ -1,59 +1,56 @@ -config: +problem: - problem: - - dimension: 2 # space is 2d - frame_structure: "PO" # keyframes have position and orientation - - first_frame: + dimension: 2 # space is 2d + frame_structure: "PO" # keyframes have position and orientation + + first_frame: + P: + mode: "factor" + state: [0,0] + noise_std: [0.31, 0.31] + O: mode: "factor" - $state: - P: [0,0] - O: [0] - $sigma: - P: [0.31, 0.31] - O: [0.31] - time_tolerance: 0.5 + state: [0] + noise_std: [0.31] - tree_manager: - type: "none" - plugin: "core" + tree_manager: + type: "none" + +solver: + max_num_iterations: 100 + verbose: 0 + period: 0.2 + interrupt_on_problem_change: false + n_threads: 2 + compute_cov: false + minimizer: LEVENBERG_MARQUARDT + use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG + function_tolerance: 0.000001 + gradient_tolerance: 0.0000000001 + +sensors: - solver: - max_num_iterations: 100 - verbose: 0 - period: 0.2 - interrupt_on_problem_change: false - n_threads: 2 - compute_cov: false - minimizer: LEVENBERG_MARQUARDT - use_nonmonotonic_steps: false # only for LEVENBERG_MARQUARDT and DOGLEG - function_tolerance: 0.000001 - gradient_tolerance: 0.0000000001 - - sensors: - - - type: "SensorOdom2d" - plugin: "core" - name: "sen odom" - follow: "demos/hello_wolf/yaml/sensor_odom_2d.yaml" # config parameters in this file + - type: "SensorOdom2d" + plugin: "core" + name: "sen odom" + follow: "sensor_odom_2d.yaml" # config parameters in this file - - type: "SensorRangeBearing" - plugin: "core" - name: "sen rb" - noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param - follow: demos/hello_wolf/yaml/sensor_range_bearing.yaml # config parameters in this file - - processors: - - - type: "ProcessorOdom2d" - plugin: "core" - name: "prc odom" - sensor_name: "sen odom" # attach processor to this sensor - follow: demos/hello_wolf/yaml/processor_odom_2d.yaml # config parameters in this file - - - type: "ProcessorRangeBearing" - plugin: "core" - name: "prc rb" - sensor_name: "sen rb" # attach processor to this sensor - follow: demos/hello_wolf/yaml/processor_range_bearing.yaml # config parameters in this file + - type: "SensorRangeBearing" + plugin: "core" + name: "sen rb" + noise_range_metres_std: 0.2 # parser only considers first appearence so the following file parsing will not overwrite this param + follow: sensor_range_bearing.yaml # config parameters in this file + +processors: + + - type: "ProcessorOdom2d" + plugin: "core" + name: "prc odom" + sensor_name: "sen odom" # attach processor to this sensor + follow: processor_odom_2d.yaml # config parameters in this file + + - type: "ProcessorRangeBearing" + plugin: "core" + name: "prc rb" + sensor_name: "sen rb" # attach processor to this sensor + follow: processor_range_bearing.yaml # config parameters in this file diff --git a/demos/hello_wolf/yaml/sensor_odom_2d.yaml b/demos/hello_wolf/yaml/sensor_odom_2d.yaml index 1480abfef..260829e09 100644 --- a/demos/hello_wolf/yaml/sensor_odom_2d.yaml +++ b/demos/hello_wolf/yaml/sensor_odom_2d.yaml @@ -2,6 +2,9 @@ k_disp_to_disp: 0.1 # m^2 / m k_rot_to_rot: 0.1 # rad^2 / rad +k_disp_to_rot: 0.0 # m^2 / rad +min_disp_var: 0.0 +min_rot_var: 0.0 apply_loss_function: true states: diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h index b7f24d4a0..31092a824 100644 --- a/include/core/capture/capture_base.h +++ b/include/core/capture/capture_base.h @@ -119,6 +119,8 @@ class CaptureBase : public NodeBase, public HasStateBlocks, public std::enable_s StateBlockConstPtr getSensorIntrinsic() const; StateBlockPtr getSensorIntrinsic(); + bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const; + void fix() override; void unfix() override; diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 33cc7d029..45aef3cff 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -39,6 +39,7 @@ class Loader; #include "core/common/wolf.h" #include "core/frame/frame_base.h" #include "core/state_block/state_block.h" +#include "core/state_block/has_state_blocks.h" #include "core/state_block/spec_composite.h" #include "core/state_block/vector_composite.h" #include "core/processor/motion_provider.h" @@ -67,28 +68,32 @@ class Problem : public std::enable_shared_from_this<Problem> friend MotionProvider; protected: - TreeManagerBasePtr tree_manager_; HardwareBasePtr hardware_ptr_; TrajectoryBasePtr trajectory_ptr_; MapBasePtr map_ptr_; + TreeManagerBasePtr tree_manager_; std::map<int, MotionProviderPtr> motion_provider_map_; - std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_; - // SizeEigen state_size_, state_cov_size_; + SizeEigen dim_; + TypeComposite frame_types_; + SpecComposite prior_options_; + bool prior_applied_; + + + std::map<std::pair<StateBlockConstPtr, StateBlockConstPtr>, Eigen::MatrixXd> covariances_; + std::map<FactorBasePtr, Notification> factor_notification_map_; std::map<StateBlockPtr, Notification> state_block_notification_map_; mutable std::mutex mut_notifications_; mutable std::mutex mut_covariances_; - //StateKeys frame_structure_; - TypeComposite frame_types_; - SpecComposite prior_options_; - bool prior_applied_; std::atomic_bool transformed_; VectorComposite transformation_; mutable std::mutex mut_transform_; - private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! + std::list<std::shared_ptr<Loader>> loaders_; + + private: // CAUTION: THESE METHODS ARE PRIVATE, DO NOT MAKE THEM PUBLIC !! Problem(const TypeComposite& _frame_structure, SizeEigen _dim); // USE create() below !! Problem(const std::string& _frame_structure, SizeEigen _dim); // USE create() below !! void setup(); @@ -100,12 +105,11 @@ class Problem : public std::enable_shared_from_this<Problem> SizeEigen _dim); // USE THIS AS A CONSTRUCTOR! static ProblemPtr autoSetup(const std::string& _input_yaml_file, const std::vector<std::string>& _primary_schema_folders={}); + static ProblemPtr autoSetup(YAML::Node _param_node); virtual ~Problem(); protected: - static void loadPlugin(const std::string& plugin_name, - const std::string& plugins_path, - std::list<std::shared_ptr<Loader>>& loaders); + void loadPlugin(const std::string& plugin_name); // Properties ----------------------------------------- public: @@ -114,7 +118,6 @@ class Problem : public std::enable_shared_from_this<Problem> TypeComposite getFrameTypes(StateKeys _keys = "") const; protected: - //void appendToFrameTypes(const StateKeys& _structure); void appendToFrameTypes(const TypeComposite& _structure); @@ -335,10 +338,8 @@ class Problem : public std::enable_shared_from_this<Problem> bool getCovarianceBlock(StateBlockConstPtr _state1, StateBlockConstPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const; bool getCovarianceBlock(std::map<StateBlockConstPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const; bool getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const; - bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const; + bool getCovariance(HasStateBlocksConstPtr _has_states_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const; bool getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& _covariance) const; - bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const; - // Solver management ---------------------------------------- diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index 9cc033cb0..0627f903b 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -186,6 +186,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh StateBlockConstPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts) const; StateBlockPtr getStateBlockDynamic(const char& _key, const TimeStamp& _ts); + bool getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const; + // Declare a state block as dynamic or static (with _dymanic = false) void setStateBlockDynamic(const char& _key, bool _dynamic = true); @@ -282,7 +284,7 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh } -#include "core/problem/problem.h" +// #include "core/problem/problem.h" #include "core/hardware/hardware_base.h" #include "core/capture/capture_base.h" #include "core/processor/processor_base.h" diff --git a/include/core/state_block/has_state_blocks.h b/include/core/state_block/has_state_blocks.h index 8ea5e94a6..a5c97899f 100644 --- a/include/core/state_block/has_state_blocks.h +++ b/include/core/state_block/has_state_blocks.h @@ -30,6 +30,7 @@ namespace wolf { +WOLF_PTR_TYPEDEFS(HasStateBlocks) class HasStateBlocks { @@ -72,8 +73,6 @@ class HasStateBlocks bool stateBlockKey(const StateBlockConstPtr& _sb, char& _key) const; StateBlockConstPtr getStateBlock(const char& _sb_key) const; StateBlockPtr getStateBlock(const char& _sb_key); - //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockConstPtr& _sb) const; - //std::unordered_map<char, StateBlockPtr>::const_iterator find(const StateBlockPtr& _sb); // Emplace derived state blocks (angle, quaternion, etc). template<typename SB, typename ... Args> diff --git a/schema/none.schema b/schema/none.schema new file mode 100644 index 000000000..d3ba158a2 --- /dev/null +++ b/schema/none.schema @@ -0,0 +1,6 @@ +type: + mandatory: false + options: ["none"] + type: string + yaml_type: scalar + doc: Nothing to say here diff --git a/schema/processor/ProcessorOdom2d.schema b/schema/processor/ProcessorOdom2d.schema new file mode 100644 index 000000000..15f0939de --- /dev/null +++ b/schema/processor/ProcessorOdom2d.schema @@ -0,0 +1,7 @@ +follow: ProcessorMotion.schema +keyframe_vote: + cov_det: + mandatory: true + type: double + yaml_type: scalar + doc: The determinant threshold of the covariance matrix of the integrated delta, to vote for a keyframe. \ No newline at end of file diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp index e6d4a1e66..0eb2cc216 100644 --- a/src/capture/capture_base.cpp +++ b/src/capture/capture_base.cpp @@ -218,6 +218,11 @@ StateBlockPtr CaptureBase::getStateBlock(const char& _key) return HasStateBlocks::getStateBlock(_key); } +bool CaptureBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const +{ + return getProblem()->getCovariance(shared_from_this(), _keys, _cov); +} + void CaptureBase::fix() { HasStateBlocks::fix(); diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index fa3c402a9..b4c0ffb24 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -132,7 +132,7 @@ void FrameBase::link(ProblemPtr _prb) bool FrameBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const { - return getProblem()->getFrameCovariance(shared_from_this(), _keys, _cov); + return getProblem()->getCovariance(shared_from_this(), _keys, _cov); } FrameBaseConstPtr FrameBase::getPreviousFrame(const unsigned int& i) const diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp index f17dc8769..f697d7482 100644 --- a/src/landmark/landmark_base.cpp +++ b/src/landmark/landmark_base.cpp @@ -102,7 +102,7 @@ void LandmarkBase::remove(bool viral_remove_empty_parent) bool LandmarkBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const { - return getProblem()->getLandmarkCovariance(shared_from_this(), _keys, _cov); + return getProblem()->getCovariance(shared_from_this(), _keys,_cov); } YAML::Node LandmarkBase::toYaml() const diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 6fa2388c1..d5139bd35 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -43,31 +43,33 @@ namespace wolf { Problem::Problem(const TypeComposite& _frame_types, SizeEigen _dim) : - tree_manager_(nullptr), hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), map_ptr_(std::make_shared<MapBase>()), + tree_manager_(nullptr), motion_provider_map_(), dim_(_dim), frame_types_(_frame_types), prior_options_(), prior_applied_(false), - transformed_(false) + transformed_(false), + loaders_() { checkTypeComposite(frame_types_, dim_); } Problem::Problem(const StateKeys& _frame_keys, SizeEigen _dim) : - tree_manager_(nullptr), hardware_ptr_(std::make_shared<HardwareBase>()), trajectory_ptr_(std::make_shared<TrajectoryBase>()), map_ptr_(std::make_shared<MapBase>()), + tree_manager_(nullptr), motion_provider_map_(), dim_(_dim), frame_types_(), prior_options_(), prior_applied_(false), - transformed_(false) + transformed_(false), + loaders_() { for (auto key : _frame_keys) { @@ -114,30 +116,6 @@ ProblemPtr Problem::create(const std::string& _frame_structure, SizeEigen _dim) ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::vector<std::string>& _primary_schema_folders) { - // Loaders - auto loaders = std::list<std::shared_ptr<Loader>>(); - std::string plugins_path = _WOLF_LIB_DIR; - if (plugins_path.back() != '/') - { - plugins_path += '/'; // only works for UNIX systems - } - // // get all plugins - // auto plugins_nodes = yaml_schema_cpp::findNodesWithKey(server.getNode(),"plugins"); - // std::set<std::string> plugins; - // for (auto plugin_n : plugins_nodes) - // { - // if (not plugin_n.IsScalar() and not yaml_schema_cpp::checkType(plugin_n, "string")) - // { - // throw std::runtime_error("Problem::autoSetup: there is one 'plugin' param that is not a string.") - // } - // // plugin string - // auto plugin_str = plugin_n.as<std::string>(); - // if (plugins.count(plugin_str) == 0) - // { - // plugins.insert(plugin_str); - // } - // } - // Schema folders std::vector<std::string> schema_folders = _primary_schema_folders; schema_folders.push_back(_WOLF_SCHEMAS_DIR); @@ -164,12 +142,15 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve WOLF_INFO("schema applied"); // Get param node - YAML::Node param_node = server.getNode(); + return Problem::autoSetup(server.getNode()); +} +ProblemPtr Problem::autoSetup(YAML::Node _param_node) +{ // PROBLEM =============================================================================== // structure and dimension WOLF_INFO("Loading problem parameters"); - YAML::Node problem_node = param_node["problem"]; + YAML::Node problem_node = _param_node["problem"]; int dim = problem_node["dimension"].as<int>(); TypeComposite frame_types; if (problem_node["frame_types"]) @@ -184,6 +165,10 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve tree_manager_type != "None" and tree_manager_type != "NONE") { + if (not FactoryTreeManager::isCreatorRegistered(tree_manager_type)) + { + problem->loadPlugin(tree_manager_node["plugin"].as<std::string>()); + } problem->setTreeManager(FactoryTreeManager::create(tree_manager_type, tree_manager_node)); } @@ -197,13 +182,13 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve // SENSORS =============================================================================== // load plugin if sensor is not registered WOLF_INFO("Loading sensors parameters"); - YAML::Node sensors_node = param_node["sensors"]; + YAML::Node sensors_node = _param_node["sensors"]; for (auto sensor_n : sensors_node) { auto sensor_type = sensor_n["type"].as<std::string>(); if (not FactorySensor::isCreatorRegistered(sensor_type)) { - loadPlugin(sensor_n["plugin"].as<std::string>(), plugins_path, loaders); + problem->loadPlugin(sensor_n["plugin"].as<std::string>()); } problem->installSensor(sensor_n); } @@ -211,13 +196,13 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve // PROCESSORS =============================================================================== // load plugin if processor is not registered WOLF_INFO("Loading processors parameters"); - YAML::Node processors_node = param_node["processors"]; + YAML::Node processors_node = _param_node["processors"]; for (auto processor_n : processors_node) { auto processor_type = processor_n["type"].as<std::string>(); if (not FactoryProcessor::isCreatorRegistered(processor_type)) { - loadPlugin(processor_n["plugin"].as<std::string>(), plugins_path, loaders); + problem->loadPlugin(processor_n["plugin"].as<std::string>()); } problem->installProcessor(processor_n); } @@ -225,16 +210,16 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve // MAP (optional) =============================================================================== // Default MapBase WOLF_INFO("Loading map parameters"); - if (not param_node["map"]) + if (not _param_node["map"]) { - param_node["map"]["type"] = "MapBase"; + _param_node["map"]["type"] = "MapBase"; } // load plugin if map is not registered - YAML::Node map_node = param_node["map"]; + YAML::Node map_node = _param_node["map"]; auto map_type = map_node["type"].as<std::string>(); if (not FactoryMap::isCreatorRegistered(map_type)) { - loadPlugin(map_node["plugin"].as<std::string>(), plugins_path, loaders); + problem->loadPlugin(map_node["plugin"].as<std::string>()); } WOLF_TRACE("Map Type: ", map_type); auto map = FactoryMap::create(map_type, map_node); @@ -246,21 +231,25 @@ ProblemPtr Problem::autoSetup(const std::string& _input_yaml_file, const std::ve return problem; } -void Problem::loadPlugin(const std::string& plugin_name, - const std::string& plugins_path, - std::list<std::shared_ptr<Loader>>& loaders) +void Problem::loadPlugin(const std::string& plugin_name) { #if __APPLE__ std::string lib_extension = ".dylib"; #else std::string lib_extension = ".so"; #endif - + + std::string plugins_path = _WOLF_LIB_DIR; + if (plugins_path.back() != '/') + { + plugins_path += '/'; // only works for UNIX systems + } + std::string plugin = plugins_path + "libwolf" + plugin_name + lib_extension; - WOLF_TRACE("Loading plugin " + plugin_name + " via " + plugin); + WOLF_TRACE("Loading plugin " + plugin_name + " in " + plugins_path); auto l = std::make_shared<LoaderRaw>(plugin); l->load(); - loaders.push_back(l); + loaders_.push_back(l); } Problem::~Problem() @@ -1004,28 +993,34 @@ bool Problem::getCovarianceBlock(StateBlockConstPtr _state, Eigen::MatrixXd& _co return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col); } -bool Problem::getFrameCovariance(FrameBaseConstPtr _frame, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const +bool Problem::getCovariance(HasStateBlocksConstPtr _has_blocks, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const { - if (not _frame->hasKeys(_keys)) + if (not _has_blocks) + { + WOLF_WARN("Problem::getCovariance: called with a nullptr"); + return false; + } + if (not _has_blocks->hasKeys(_keys)) { + WOLF_WARN(" does not have the keys ", _keys, ". Available keys: ", _has_blocks->getKeys()); return false; } bool success(true); // resizing - SizeEigen sz = _frame->getLocalSize(); + SizeEigen sz = _has_blocks->getLocalSize(); _covariance.resize(sz, sz); // filling covariance int i = 0, j = 0; for (auto key_i : _keys) { - auto sb_i = _frame->getStateBlock(key_i); + auto sb_i = _has_blocks->getStateBlock(key_i); j = 0; for (auto key_j : _keys) { - auto sb_j = _frame->getStateBlock(key_j); + auto sb_j = _has_blocks->getStateBlock(key_j); success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); j += sb_j->getLocalSize(); } @@ -1037,38 +1032,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame, const StateKeys& _key bool Problem::getLastFrameCovariance(const StateKeys& _keys, Eigen::MatrixXd& cov) const { - auto frm = getLastFrame(); - return getFrameCovariance(frm, _keys, cov); -} - -bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark, const StateKeys& _keys, Eigen::MatrixXd& _covariance) const -{ - if (not _landmark->hasKeys(_keys)) - { - return false; - } - bool success(true); - - // resizing - SizeEigen sz = _landmark->getLocalSize(); - _covariance.resize(sz, sz); - - // filling covariance - int i = 0, j = 0; - for (auto key_i : _keys) - { - auto sb_i = _landmark->getStateBlock(key_i); - j = 0; - for (auto key_j : _keys) - { - auto sb_j = _landmark->getStateBlock(key_j); - success = success && getCovarianceBlock(sb_i, sb_j, _covariance, i, j); - j += sb_j->getLocalSize(); - } - i += sb_i->getLocalSize(); - } - - return success; + return getCovariance(getLastFrame(), _keys, cov); } MapBaseConstPtr Problem::getMap() const diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp index 10503f263..0db3b19e0 100644 --- a/src/sensor/sensor_base.cpp +++ b/src/sensor/sensor_base.cpp @@ -365,6 +365,11 @@ StateBlockPtr SensorBase::getIntrinsic() return getStateBlockDynamic('I'); } +bool SensorBase::getCovariance(const StateKeys& _keys, Eigen::MatrixXd& _cov) const +{ + return getProblem()->getCovariance(shared_from_this(), _keys, _cov); +} + bool SensorBase::process(const CaptureBasePtr capture_ptr) { capture_ptr->setSensor(shared_from_this()); diff --git a/test/gtest_odom_2d.cpp b/test/gtest_odom_2d.cpp index 9d46f7b2a..10b60efa2 100644 --- a/test/gtest_odom_2d.cpp +++ b/test/gtest_odom_2d.cpp @@ -107,7 +107,7 @@ void show(const ProblemPtr& problem) } cout << " state: \n" << F->getStateVector("PO").transpose() << endl; Eigen::MatrixXd cov; - problem->getFrameCovariance(F, "PO", cov); + F->getCovariance("PO", cov); cout << " covariance: \n" << cov << endl; } } @@ -181,9 +181,9 @@ TEST(Odom2d, FactorFix_and_FactorOdom2d) // get covariances MatrixXd P0_solver, P1_solver, P2_solver; - ASSERT_TRUE(Pr->getFrameCovariance(F0, "PO", P0_solver)); - ASSERT_TRUE(Pr->getFrameCovariance(F1, "PO", P1_solver)); - ASSERT_TRUE(Pr->getFrameCovariance(F2, "PO", P2_solver)); + ASSERT_TRUE(F0->getCovariance("PO", P0_solver)); + ASSERT_TRUE(F1->getCovariance("PO", P1_solver)); + ASSERT_TRUE(F2->getCovariance("PO", P2_solver)); ASSERT_POSE2d_APPROX(F0->getStateVector("PO"), Vector3d(0,0,0), 1e-6); auto s0_vector = s0.vector("PO"); @@ -508,13 +508,13 @@ TEST(Odom2d, KF_callback) // check the split KF MatrixXd KF1_cov; - ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, "PO", KF1_cov)); + ASSERT_TRUE(keyframe_1->getCovariance("PO", KF1_cov)); ASSERT_POSE2d_APPROX(keyframe_1->getStateVector("PO"), integrated_pose_vector[m_split], 1e-6); ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6); // check other KF in the future of the split KF MatrixXd KF2_cov; - ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, "PO", KF2_cov)); + ASSERT_TRUE(keyframe_2->getCovariance("PO", KF2_cov)); ASSERT_POSE2d_APPROX(problem->getLastFrame()->getStateVector("PO"), integrated_pose_vector[n_split], 1e-6); ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6); diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index 23cf399b4..a70aa857d 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -413,7 +413,7 @@ TEST(Problem, Covariances) // get covariance Eigen::MatrixXd Cov; - ASSERT_TRUE(P->getFrameCovariance(F, "PO", Cov)); + ASSERT_TRUE(F->getCovariance("PO", Cov)); ASSERT_EQ(Cov.cols() , 6); ASSERT_EQ(Cov.rows() , 6); -- GitLab