diff --git a/include/base/converter.h b/include/base/converter.h index 2bff920a96c853c460a6aabc3fd863ecc1c6212c..1824bbf870095023d17789374d4e244bf9e2434a 100644 --- a/include/base/converter.h +++ b/include/base/converter.h @@ -33,15 +33,6 @@ namespace utils{ } static inline std::vector<std::string> pairSplitter(std::string val){ std::regex exp("(\\{[^\\{:]:.*?\\})"); - // std::smatch res; - // string val = "[{x:1},{y:[1,23,3]},{z:3}]"; - // auto v = std::vector<std::string>(); - // std::string::const_iterator searchStart( val.cbegin() ); - // while ( regex_search( searchStart, val.cend(), res, exp ) ) { - // v.push_back(res[0]); - // searchStart = res.suffix().first; - // } - // return v; return getMatches(val, exp); } static inline std::array<std::string,2> splitMatrixStringRepresentation(std::string matrix){ @@ -151,19 +142,6 @@ struct converter<std::string>{ static std::string convert(double val){ return std::to_string(val); } - // template<typename A> - // static std::string convert(std::vector<A> val){ - // std::string result = ""; - // bool first = true; - // for(auto it : val){ - // if(not first) result += "," + converter<std::string>::convert(it); - // else{ - // first = false; - // result = converter<std::string>::convert(it); - // } - // } - // return "[" + result + "]"; - // } template<typename A> static std::string convert(utils::list<A> val){ std::string result = ""; @@ -210,52 +188,23 @@ struct converter<std::pair<A,B>>{ 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); } - static std::vector<A> convert(std::vector<std::string> val){ - std::vector<A> rtn = std::vector<A>(); - for(auto it : val) - rtn.push_back(converter<A>::convert(it)); - return rtn; - } }; -// template<typename A> -// struct converter<std::vector<A>>{ -// static std::vector<A> convert(std::string val){ -// auto aux = utils:: -// std::vector<A> rtn = std::vector<A>(); -// for(auto it : aux) -// rtn.push_back(converter<A>::convert(it)); -// return rtn; -// } -// static std::vector<A> convert(std::vector<std::string> val){ -// std::vector<A> rtn = std::vector<A>(); -// for(auto it : val) -// rtn.push_back(converter<A>::convert(it)); -// return rtn; -// } -// }; - // 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, int _size> -struct converter<std::array<A, _size>>{ - static std::array<A, _size> convert(std::string val){ +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,_size> rtn = std::array<A, _size>(); - if(_size != aux.size()) throw std::runtime_error("Error in trying to transform literal string to Array. Invalid argument size. Required size " - + std::to_string(_size) + "; provided size " + std::to_string(aux.size())); - for (int i = 0; i < _size; ++i) { + 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; } - // static std::vector<A> convert(std::vector<std::string> val){ - // std::vector<A> rtn = std::vector<A>(); - // for(auto it : val) - // rtn.push_back(converter<A>::convert(it)); - // 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>>{ @@ -299,11 +248,5 @@ struct converter<std::map<std::string,A>>{ return map; } }; - // template<typename A> - // struct converter<utils::toString<A>>{ - // static std::string convert(T val){ - // return std::to_string(val); - // } - // } } #endif \ No newline at end of file diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h index 64d97861c8915db1e1ea5b29f9022c8361cc2b46..671df79d4a9727126e48dc88a6fefc07d02057a7 100644 --- a/include/base/processor/processor_IMU.h +++ b/include/base/processor/processor_IMU.h @@ -11,12 +11,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsIMU); struct ProcessorParamsIMU : public ProcessorParamsMotion { - ProcessorParamsIMU() = default; - ProcessorParamsIMU(std::string _unique_name, const paramsServer& _server): - ProcessorParamsMotion(_unique_name, _server) - { - // - } + // ProcessorParamsIMU() = default; + using ProcessorParamsMotion::ProcessorParamsMotion; }; WOLF_PTR_TYPEDEFS(ProcessorIMU); diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h index fb5c9e64739ec1b68190c2bc719d2772f241ae23..1cc531a83703e61237ee7a50a8e879be48c68001 100644 --- a/include/base/processor/processor_base.h +++ b/include/base/processor/processor_base.h @@ -106,7 +106,7 @@ class PackKeyFrameBuffer * * Derive from this struct to create structs of processor parameters. */ -struct ProcessorParamsBase +struct ProcessorParamsBase : public ParamsBase { ProcessorParamsBase() = default; ProcessorParamsBase(bool _voting_active, @@ -116,7 +116,8 @@ struct ProcessorParamsBase { // } - ProcessorParamsBase(std::string _unique_name, const paramsServer& _server) + ProcessorParamsBase(std::string _unique_name, const paramsServer& _server): + ParamsBase(_unique_name, _server) { voting_active = _server.getParam<bool>(_unique_name + "/voting_active", "false"); time_tolerance = _server.getParam<Scalar>(_unique_name + "/time_tolerance", "0"); diff --git a/include/base/processor/processor_frame_nearest_neighbor_filter.h b/include/base/processor/processor_frame_nearest_neighbor_filter.h index be1205e27988461473786627ca5e0d3acc62748f..3f28b915dbe77793d579342ef13e5d3133ee2cef 100644 --- a/include/base/processor/processor_frame_nearest_neighbor_filter.h +++ b/include/base/processor/processor_frame_nearest_neighbor_filter.h @@ -4,6 +4,7 @@ // Wolf related headers #include "base/processor/processor_loopclosure_base.h" #include "base/state_block.h" +#include "base/params_server.hpp" namespace wolf{ @@ -38,7 +39,20 @@ struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClo { // } - + ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server): + ProcessorParamsLoopClosure(_unique_name, _server) + { + buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size", ""); + sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree", ""); + auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type", ""); + if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE; + else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE; + else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID; + else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID; + else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE; + else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str); + probability_ = _server.getParam<Scalar>(_unique_name + "/probability", ""); + } virtual ~ProcessorParamsFrameNearestNeighborFilter() = default; int buffer_size_; diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h index 1ded95bf036f2569e95f1ec9f41b5141789f70d5..76298ca2edf0a01cdeb6dba4f5a005c608d6b157 100644 --- a/include/base/processor/processor_loopclosure_base.h +++ b/include/base/processor/processor_loopclosure_base.h @@ -10,6 +10,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsLoopClosure); struct ProcessorParamsLoopClosure : public ProcessorParamsBase { + using ProcessorParamsBase::ProcessorParamsBase; // virtual ~ProcessorParamsLoopClosure() = default; // add neccesery parameters for loop closure initialisation here and initialize diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h index a8a8942c73218571677b6653fdbf994645fba84f..49fef092775ee678cf74f2e183f56b1a563684f7 100644 --- a/include/base/processor/processor_odom_2D.h +++ b/include/base/processor/processor_odom_2D.h @@ -15,7 +15,7 @@ #include "base/params_server.hpp" namespace wolf { - + WOLF_PTR_TYPEDEFS(ProcessorOdom2D); WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsOdom2D); @@ -25,14 +25,13 @@ struct ProcessorParamsOdom2D : public ProcessorParamsMotion Scalar unmeasured_perturbation_std = 0.001; // no particular dimension: the same for displacement and angle ProcessorParamsOdom2D() = default; - ProcessorParamsOdom2D(std::string _unique_name, const paramsServer& _server): + ProcessorParamsOdom2D(std::string _unique_name, const wolf::paramsServer & _server): ProcessorParamsMotion(_unique_name, _server) { - cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); - unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001"); + cov_det = _server.getParam<Scalar>(_unique_name + "/cov_det", "1.0"); + unmeasured_perturbation_std = _server.getParam<Scalar>(_unique_name + "/unmeasured_perturbation_std", "0.001"); } }; - class ProcessorOdom2D : public ProcessorMotion { public: diff --git a/include/base/processor/processor_params_image.h b/include/base/processor/processor_params_image.h index 6ce3eaace3119ed1e68725bb33d2ca0df4684bbb..5d200dd0835e5018adeb8a900cb708824326ae5e 100644 --- a/include/base/processor/processor_params_image.h +++ b/include/base/processor/processor_params_image.h @@ -4,6 +4,7 @@ // wolf #include "base/processor/processor_tracker_feature.h" #include "base/processor/processor_tracker_landmark.h" +#include "base/params_server.hpp" namespace wolf { @@ -19,6 +20,16 @@ struct ProcessorParamsTrackerFeatureImage : public ProcessorParamsTrackerFeature Scalar pixel_noise_std; ///< std noise of the pixel Scalar pixel_noise_var; ///< var noise of the pixel + ProcessorParamsTrackerFeatureImage() = default; + ProcessorParamsTrackerFeatureImage(std::string _unique_name, const paramsServer& _server): + ProcessorParamsTrackerFeature(_unique_name, _server) + { + yaml_file_params_vision_utils = _server.getParam<std::string>(_unique_name + "/yaml_file_params_vision_utils", ""); + min_response_for_new_features = _server.getParam<Scalar>(_unique_name + "/min_response_for_new_features", ""); + distance = _server.getParam<Scalar>(_unique_name + "/distance", ""); + pixel_noise_std = _server.getParam<Scalar>(_unique_name + "/pixel_noise_std", ""); + pixel_noise_var = _server.getParam<Scalar>(_unique_name + "/pixel_noise_var", ""); + } }; WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmarkImage); @@ -32,6 +43,17 @@ struct ProcessorParamsTrackerLandmarkImage : public ProcessorParamsTrackerLandma Scalar pixel_noise_std; ///< std noise of the pixel Scalar pixel_noise_var; ///< var noise of the pixel + ProcessorParamsTrackerLandmarkImage(std::string _unique_name, const paramsServer& _server): + ProcessorParamsTrackerLandmark(_unique_name, _server) + { + yaml_file_params_vision_utils = _server.getParam<std::string>(_unique_name + "/yaml_file_params_vision_utils", ""); + + min_response_for_new_features = _server.getParam<Scalar>(_unique_name + "/min_response_for_new_features", ""); + distance = _server.getParam<Scalar>(_unique_name + "/distance", ""); + + pixel_noise_std = _server.getParam<Scalar>(_unique_name + "/pixel_noise_std", ""); + pixel_noise_var = _server.getParam<Scalar>(_unique_name + "/pixel_noise_var", ""); + } }; } diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h index f132626d7a028855fbaae0856e09c0995665f9f9..2b7bf99530d50aa8b9f26a2c67ac340b48e341da 100644 --- a/include/base/processor/processor_tracker.h +++ b/include/base/processor/processor_tracker.h @@ -12,13 +12,20 @@ #include "base/capture/capture_motion.h" namespace wolf { - + WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTracker); struct ProcessorParamsTracker : public ProcessorParamsBase { unsigned int min_features_for_keyframe; ///< minimum nbr. of features to vote for keyframe unsigned int max_new_features; + ProcessorParamsTracker() = default; + ProcessorParamsTracker(std::string _unique_name, const wolf::paramsServer & _server): + ProcessorParamsBase(_unique_name, _server) + { + min_features_for_keyframe = _server.getParam<unsigned int>(_unique_name + "/min_features_for_keyframe", "1"); + max_new_features = _server.getParam<unsigned int>(_unique_name + "/max_new_features", "10"); + } }; WOLF_PTR_TYPEDEFS(ProcessorTracker); diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h index 52bd493f6870a1424621f5085ebed10b9558df60..5970458afc551240c34c165dc6db1b6e98e59277 100644 --- a/include/base/processor/processor_tracker_feature.h +++ b/include/base/processor/processor_tracker_feature.h @@ -22,9 +22,9 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeature); struct ProcessorParamsTrackerFeature : public ProcessorParamsTracker { - // + using ProcessorParamsTracker::ProcessorParamsTracker; }; - + WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature); /** \brief Feature tracker processor diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h index 5bc33bf47a51b4b267c64ecf916ccc271047c280..1a82246fc6829d4a8b7398a3536d4366d6de120c 100644 --- a/include/base/processor/processor_tracker_feature_corner.h +++ b/include/base/processor/processor_tracker_feature_corner.h @@ -17,6 +17,7 @@ #include "base/state_block.h" #include "base/association/association_tree.h" #include "base/processor/processor_tracker_feature.h" +#include "base/params_server.hpp" //laser_scan_utils //#include "laser_scan_utils/scan_basics.h" diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h index dca3b791a6885b5e89296b6f988838a511c212fa..929ea44178e5309fba817a6325fae7489fae1259 100644 --- a/include/base/processor/processor_tracker_feature_trifocal.h +++ b/include/base/processor/processor_tracker_feature_trifocal.h @@ -4,6 +4,7 @@ //Wolf includes #include "base/processor/processor_tracker_feature.h" #include "base/capture/capture_image.h" +#include "base/params_server.hpp" // Vision utils #include <vision_utils.h> @@ -27,6 +28,18 @@ struct ProcessorParamsTrackerFeatureTrifocal : public ProcessorParamsTrackerFeat Scalar max_euclidean_distance; Scalar pixel_noise_std; ///< std noise of the pixel int min_track_length_for_constraint; ///< Minimum track length of a matched feature to create a constraint + ProcessorParamsTrackerFeatureTrifocal() = default; + ProcessorParamsTrackerFeatureTrifocal(std::string _unique_name, const paramsServer& _server): + ProcessorParamsTrackerFeature(_unique_name, _server) + { + yaml_file_params_vision_utils = _server.getParam<std::string>(_unique_name + "/yaml_file_params_vision_utils", ""); + n_cells_h = _server.getParam<int>(_unique_name + "/n_cells_h", ""); + n_cells_v = _server.getParam<int>(_unique_name + "/n_cells_v", ""); + min_response_new_feature = _server.getParam<int>(_unique_name + "/min_response_new_feature", ""); + max_euclidean_distance = _server.getParam<Scalar>(_unique_name + "/max_euclidean_distance", ""); + pixel_noise_std = _server.getParam<Scalar>(_unique_name + "/pixel_noise_std", ""); + min_track_length_for_constraint = _server.getParam<int>(_unique_name + "/min_track_length_for_constraint", ""); + } }; WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureTrifocal); diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h index 293570afa29406dfc8515ef9c7d684af2da35cf9..55fa26fe735587674b0599e9ec8057c636c2cd27 100644 --- a/include/base/processor/processor_tracker_landmark.h +++ b/include/base/processor/processor_tracker_landmark.h @@ -20,6 +20,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerLandmark); struct ProcessorParamsTrackerLandmark : public ProcessorParamsTracker { + using ProcessorParamsTracker::ProcessorParamsTracker; // }; diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h index 01df97dc63db1cbd6a02abce6625dc6b24247d47..55961ad795c3867d081f90274e3dabc60e6c6c0a 100644 --- a/include/base/sensor/sensor_base.h +++ b/include/base/sensor/sensor_base.h @@ -21,9 +21,10 @@ namespace wolf { * * Derive from this struct to create structs of sensor intrinsic parameters. */ -struct IntrinsicsBase +struct IntrinsicsBase: public ParamsBase { virtual ~IntrinsicsBase() = default; + using ParamsBase::ParamsBase; }; class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBase> diff --git a/include/base/sensor/sensor_odom_2D.h b/include/base/sensor/sensor_odom_2D.h index 9563c39434bd5495e5939ec3a2f117fe46ae93e6..798d1bbdd314e8e70725dd1f135c2c117f6e9850 100644 --- a/include/base/sensor/sensor_odom_2D.h +++ b/include/base/sensor/sensor_odom_2D.h @@ -1,4 +1,3 @@ - #ifndef SENSOR_ODOM_2D_H_ #define SENSOR_ODOM_2D_H_ @@ -31,21 +30,21 @@ class SensorOdom2D : public SensorBase SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics); virtual ~SensorOdom2D(); - + /** \brief Returns displacement noise factor - * + * * Returns displacement noise factor - * - **/ + * + **/ Scalar getDispVarToDispNoiseFactor() const; /** \brief Returns rotation noise factor - * + * * Returns rotation noise factor - * - **/ + * + **/ Scalar getRotVarToRotNoiseFactor() const; - + public: static SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsBasePtr _intrinsics); diff --git a/include/base/wolf.h b/include/base/wolf.h index d642f730a22538e9bfa33c6d6d12544a5c82e381..b7d85fc000a9ea8932e0b3b371f58b64f2bf0239 100644 --- a/include/base/wolf.h +++ b/include/base/wolf.h @@ -11,6 +11,7 @@ // Enable project-specific definitions and macros #include "internal/config.h" #include "base/logging.h" +#include "base/params_server.hpp" //includes from Eigen lib #include <Eigen/Dense> @@ -366,7 +367,16 @@ bool makePosDef(Eigen::Matrix<T,N,N,RC>& M, const T& eps = Constants::EPS) } //=================================================== +struct ParamsBase +{ + ParamsBase() = default; + ParamsBase(std::string _unique_name, const paramsServer&) + { + // + } + virtual ~ParamsBase() = default; +}; } // namespace wolf #endif /* WOLF_H_ */ diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp index 4901170c2e99579a6fbe3618a261f4917bcd472a..23c1dbc6800bc02e2fd9f2c7660220e0b26e78d2 100644 --- a/src/processor/processor_frame_nearest_neighbor_filter.cpp +++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -2,7 +2,6 @@ namespace wolf { - ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF): ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF), params_NNF(_params_NNF) diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp index 6dca7003b2f5a1745b992ed2319b39459afd0538..bcbc73b20bf870ed3ab583e3b225d07c17e671ce 100644 --- a/src/processor/processor_odom_2D.cpp +++ b/src/processor/processor_odom_2D.cpp @@ -188,7 +188,6 @@ ProcessorBasePtr ProcessorOdom2D::create(const std::string& _unique_name, const return prc_ptr; } - ProcessorBasePtr ProcessorOdom2D::createNew(const std::string& _unique_name, const paramsServer& _server, const SensorBasePtr sensor_ptr) {