diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp index c23a9c6625c66118f37b1051c24934c3f22aedf7..b1531f45173b200d2c10bb448996ec4c87095263 100644 --- a/hello_wolf/hello_wolf.cpp +++ b/hello_wolf/hello_wolf.cpp @@ -111,7 +111,7 @@ int main() CeresManagerPtr ceres = std::make_shared<CeresManager>(problem, options); // sensor odometer 2D - IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>(); + ParamsSensorOdom2DPtr intrinsics_odo = std::make_shared<ParamsSensorOdom2D>(); SensorBasePtr sensor_odo = problem->installSensor("SensorOdom2D", "sensor odo", Vector3d(0,0,0), intrinsics_odo); // processor odometer 2D @@ -126,7 +126,7 @@ int main() ProcessorBasePtr processor = problem->installProcessor("ProcessorOdom2D", "processor odo", sensor_odo, params_odo); // sensor Range and Bearing - IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>(); + ParamsSensorRangeBearingPtr intrinsics_rb = std::make_shared<ParamsSensorRangeBearing>(); intrinsics_rb->noise_range_metres_std = 0.1; intrinsics_rb->noise_bearing_degrees_std = 1.0; SensorBasePtr sensor_rb = problem->installSensor("SensorRangeBearing", "sensor RB", Vector3d(1,1,0), intrinsics_rb); diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp index c7e3dd536cc52dad3735bd0cde493706db8eefda..f2348928a25a2bc8835ea61f3bad7c1139424d7c 100644 --- a/hello_wolf/sensor_range_bearing.cpp +++ b/hello_wolf/sensor_range_bearing.cpp @@ -23,7 +23,7 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D"); } -SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const IntrinsicsRangeBearingPtr _intr) : +SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr) : SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std))) { // diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h index 75f9a3d8a9b66f096ab8bbc2085ca271efeb04fd..e62bfc1a5cd4ff7115cac06f9738204c71017f2b 100644 --- a/hello_wolf/sensor_range_bearing.h +++ b/hello_wolf/sensor_range_bearing.h @@ -13,25 +13,25 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsRangeBearing); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorRangeBearing); -struct IntrinsicsRangeBearing : public IntrinsicsBase +struct ParamsSensorRangeBearing : public ParamsSensorBase { double noise_range_metres_std = 0.05; double noise_bearing_degrees_std = 0.5; - IntrinsicsRangeBearing() + ParamsSensorRangeBearing() { //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. } - IntrinsicsRangeBearing(std::string _unique_name, const ParamsServer& _server): - IntrinsicsBase(_unique_name, _server) + ParamsSensorRangeBearing(std::string _unique_name, const ParamsServer& _server): + ParamsSensorBase(_unique_name, _server) { noise_range_metres_std = _server.getParam<double>(prefix + _unique_name + "/noise_range_metres_std"); noise_bearing_degrees_std = _server.getParam<double>(prefix + _unique_name + "/noise_bearing_degrees_std"); } std::string print() const { - return "" + IntrinsicsBase::print() + "\n" + return "" + ParamsSensorBase::print() + "\n" + "noise_range_metres_std: " + std::to_string(noise_range_metres_std) + "\n" + "noise_bearing_degrees_std: " + std::to_string(noise_bearing_degrees_std) + "\n"; } @@ -43,8 +43,8 @@ class SensorRangeBearing : public SensorBase { public: SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std); - SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const IntrinsicsRangeBearingPtr _intr); - WOLF_SENSOR_CREATE(SensorRangeBearing, IntrinsicsRangeBearing, 3); + SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const ParamsSensorRangeBearingPtr _intr); + WOLF_SENSOR_CREATE(SensorRangeBearing, ParamsSensorRangeBearing, 3); virtual ~SensorRangeBearing(); }; diff --git a/include/core/common/factory.h b/include/core/common/factory.h index cb19d7a67db300183bd3579b9c9acc2cdd31524a..88820c8b382fed793ba0cdc0db73a60eafcd44ca 100644 --- a/include/core/common/factory.h +++ b/include/core/common/factory.h @@ -42,7 +42,7 @@ namespace wolf * * For example, you may use as __TypeBase__ the following types: * - LandmarkBase: the Factory creates landmarks deriving from LandmarkBase and returns base pointers ````LandmarkBasePtr```` to them - * - IntrinsicsBase: the Factory creates intrinsic parameters deriving from IntrinsicsBase and returns base pointers ````IntrinsicsBasePtr```` to them + * - ParamsSensorBase: the Factory creates intrinsic parameters deriving from ParamsSensorBase and returns base pointers ````ParamsSensorBasePtr```` to them * - XxxBase: the Factory creates objects deriving from XxxBase and returns pointers ````XxxBasePtr```` to them. * * - The class in also templatized on the type of the input parameter of the creator, __TypeInput__: @@ -77,8 +77,8 @@ namespace wolf * #### Define correct TYPE names * The rule to make new TYPE strings unique is that you skip the generic 'Type' prefix from your class name, * and you build a string in CAPITALS with space separators, e.g.: - * - IntrinsicsCamera -> ````"CAMERA"```` - * - IntrinsicsLaser2D -> ````"LASER 2D"```` + * - ParamsSensorCamera -> ````"CAMERA"```` + * - ParamsSensorLaser2D -> ````"LASER 2D"```` * - LandmarkPolyline2D -> ````"POLYLINE 2D"```` * - etc. * @@ -86,7 +86,7 @@ namespace wolf * The first thing to know is that we have defined typedefs for the templates that we are using. For example: * * \code - * typedef Factory<IntrinsicsBase, std::string> IntrinsicsFactory; + * typedef Factory<ParamsSensorBase, std::string> ParamsSensorFactory; * typedef Factory<ProcessorParamsBase, std::string> ProcessorParamsFactory; * typedef Factory<LandmarkBase, YAML::Node> LandmarkFactory; * \endcode @@ -122,7 +122,7 @@ namespace wolf * Two examples: * * \code - * static IntrinsicsBasePtr create(const std::string& _intrinsics_dot_yaml) + * static ParamsSensorBasePtr create(const std::string& _intrinsics_dot_yaml) * static LandmarkBasePtr create(const YAML::Node& _lmk_yaml_node) * \endcode * @@ -145,10 +145,10 @@ namespace wolf * For example, in sensor_camera_yaml.cpp we find the line: * * \code - * const bool registered_camera_intr = IntrinsicsFactory::get().registerCreator("CAMERA", createIntrinsicsCamera); + * const bool registered_camera_intr = ParamsSensorFactory::get().registerCreator("CAMERA", createParamsSensorCamera); * \endcode * - * which is a static invocation (i.e., it is placed at global scope outside of the IntrinsicsCamera class). + * which is a static invocation (i.e., it is placed at global scope outside of the ParamsSensorCamera class). * * Therefore, at application level, all objects that have a .cpp file compiled are automatically registered. * @@ -218,7 +218,7 @@ namespace wolf * \endcode * * ### More information - * - IntrinsicsFactory: typedef of this template to create intrinsic structs deriving from IntrinsicsBase directly from YAML files. + * - ParamsSensorFactory: typedef of this template to create intrinsic structs deriving from ParamsSensorBase directly from YAML files. * - ProcessorParamsFactory: typedef of this template to create processor params structs deriving from ProcessorParamsBase directly from YAML files. * - LandmarkFactory: typedef of this template to create landmarks deriving from LandmarkBase directly from YAML nodes. * - Problem::loadMap() : to load a maps directly from YAML files. diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h index 5db953501ad44029b995ba8ba15f516209a02c6c..9f548fd325de45d8901f432975bf23563938c988 100644 --- a/include/core/common/wolf.h +++ b/include/core/common/wolf.h @@ -215,8 +215,8 @@ WOLF_PTR_TYPEDEFS(HardwareBase); WOLF_PTR_TYPEDEFS(SensorBase); WOLF_LIST_TYPEDEFS(SensorBase); -// - - Intrinsics -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsBase); +// - - ParamsSensor +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorBase); // - Processors WOLF_PTR_TYPEDEFS(ProcessorBase); diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h index 362235b95fed754822e35e8e7082258d5bc194a6..2d2f0e572e9b0ce32729d4a9da4b4e2e135206fd 100644 --- a/include/core/problem/problem.h +++ b/include/core/problem/problem.h @@ -10,7 +10,7 @@ class MapBase; class ProcessorMotion; class StateBlock; class TimeStamp; -struct IntrinsicsBase; +struct ParamsSensorBase; struct ProcessorParamsBase; } @@ -84,13 +84,13 @@ class Problem : public std::enable_shared_from_this<Problem> SensorBasePtr installSensor(const std::string& _sen_type, // const std::string& _unique_sensor_name, // const Eigen::VectorXd& _extrinsics, // - IntrinsicsBasePtr _intrinsics = nullptr); + ParamsSensorBasePtr _intrinsics = nullptr); /** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file * \param _sen_type type of sensor * \param _unique_sensor_name unique sensor name, used to identify the particular instance of the sensor * \param _extrinsics a vector of extrinsic parameters: size 2 for 2D position, 3 for 2D pose, 3 for 3D position, 7 for 3D pose. - * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in IntrinsicsFactory under the key _sen_type. + * \param _intrinsics_filename the name of a file containing the intrinsic parameters in a format compatible with the intrinsics creator registered in ParamsSensorFactory under the key _sen_type. */ SensorBasePtr installSensor(const std::string& _sen_type, // const std::string& _unique_sensor_name, // diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h index ab90bb26d654e76905e47a9b57effd6be4b69e54..a5afc4664a826c19b3c58272164b8e3849d1c15c 100644 --- a/include/core/sensor/sensor_base.h +++ b/include/core/sensor/sensor_base.h @@ -29,11 +29,11 @@ namespace wolf { * In order to use this macro, the derived sensor class, SensorClass, * must have a constructor available with the API: * - * SensorClass(const VectorXd & _extrinsics, const IntrinsicsClassPtr _intrinsics); + * SensorClass(const VectorXd & _extrinsics, const ParamsSensorClassPtr _intrinsics); * * We recommend writing one of such constructors in your derived sensors. */ -#define WOLF_SENSOR_CREATE(SensorClass, IntrinsicsClass, ExtrinsicsSize) \ +#define WOLF_SENSOR_CREATE(SensorClass, ParamsSensorClass, ExtrinsicsSize) \ static \ SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \ { \ @@ -41,7 +41,7 @@ SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _serve \ assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ \ - auto params = std::make_shared<IntrinsicsClass>(_unique_name, _server); \ + auto params = std::make_shared<ParamsSensorClass>(_unique_name, _server); \ \ auto sensor = std::make_shared<SensorClass>(extrinsics, params); \ \ @@ -51,11 +51,11 @@ SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _serve } \ \ static \ -SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const IntrinsicsBasePtr _intrinsics) \ +SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const ParamsSensorBasePtr _intrinsics) \ { \ assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \ \ - auto params = std::static_pointer_cast<IntrinsicsClass>(_intrinsics); \ + auto params = std::static_pointer_cast<ParamsSensorClass>(_intrinsics); \ \ auto sensor = std::make_shared<SensorClass>(_extrinsics, params); \ \ @@ -71,10 +71,10 @@ SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _ex * * Derive from this struct to create structs of sensor intrinsic parameters. */ -struct IntrinsicsBase: public ParamsBase +struct ParamsSensorBase: public ParamsBase { std::string prefix = "sensor/"; - virtual ~IntrinsicsBase() = default; + virtual ~ParamsSensorBase() = default; using ParamsBase::ParamsBase; std::string print() const { diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h index 8c0a0ad3b2a745deb49e29273e162bd449f1993c..f84227fc6f35697158272b5bbe7861c95f7be2f0 100644 --- a/include/core/sensor/sensor_diff_drive.h +++ b/include/core/sensor/sensor_diff_drive.h @@ -13,9 +13,9 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsDiffDrive); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorDiffDrive); -struct IntrinsicsDiffDrive : public IntrinsicsBase +struct ParamsSensorDiffDrive : public ParamsSensorBase { double radius_left; double radius_right; @@ -25,9 +25,9 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase Eigen::Vector3d prior_cov_diag; double ticks_cov_factor; - IntrinsicsDiffDrive() = default; - IntrinsicsDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server) - : IntrinsicsBase(_unique_name, _server) + ParamsSensorDiffDrive() = default; + ParamsSensorDiffDrive(std::string _unique_name, const wolf::ParamsServer& _server) + : ParamsSensorBase(_unique_name, _server) { radius_left = _server.getParam<double>(prefix + _unique_name + "/radius_left"); radius_right = _server.getParam<double>(prefix + _unique_name + "/radius_right"); @@ -39,7 +39,7 @@ struct IntrinsicsDiffDrive : public IntrinsicsBase } std::string print() const { - return "\n" + IntrinsicsBase::print() + "\n" + return "\n" + ParamsSensorBase::print() + "\n" + "radius_left: " + std::to_string(radius_left) + "\n" + "radius_right: " + std::to_string(radius_right) + "\n" + "wheel_separation: " + std::to_string(wheel_separation) + "\n" @@ -56,10 +56,10 @@ class SensorDiffDrive : public SensorBase { public: SensorDiffDrive(const Eigen::VectorXd& _extrinsics, - IntrinsicsDiffDrivePtr _intrinsics); - WOLF_SENSOR_CREATE(SensorDiffDrive, IntrinsicsDiffDrive, 3); + ParamsSensorDiffDrivePtr _intrinsics); + WOLF_SENSOR_CREATE(SensorDiffDrive, ParamsSensorDiffDrive, 3); virtual ~SensorDiffDrive(); - IntrinsicsDiffDriveConstPtr getParams() const; + ParamsSensorDiffDriveConstPtr getParams() const; double getRadiansPerTick() const { @@ -67,12 +67,12 @@ class SensorDiffDrive : public SensorBase } protected: - IntrinsicsDiffDrivePtr params_diff_drive_; + ParamsSensorDiffDrivePtr params_diff_drive_; double radians_per_tick; ///< Not user-definable -- DO NOT PRETEND TO USE YAML TO SET THIS PARAM. }; -inline wolf::IntrinsicsDiffDriveConstPtr SensorDiffDrive::getParams() const +inline wolf::ParamsSensorDiffDriveConstPtr SensorDiffDrive::getParams() const { return params_diff_drive_; } diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h index 3521a63c91c2e69086876401a57745371d480819..1f0b803df4febb6c3b4f71b1f77f60a4909d76ca 100644 --- a/include/core/sensor/sensor_factory.h +++ b/include/core/sensor/sensor_factory.h @@ -11,7 +11,7 @@ namespace wolf { class SensorBase; -struct IntrinsicsBase; +struct ParamsSensorBase; } // wolf @@ -75,11 +75,11 @@ namespace wolf * The method SensorCamera::create() exists in the SensorCamera class as a static method. * All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type. * This API includes a sensor name, a vector of extrinsic parameters, - * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr, + * and a pointer to a base struct of intrinsic parameters, ParamsSensorBasePtr, * that can be derived for each derived sensor: * * \code - * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics) * \endcode * * See further down for an implementation example. @@ -116,7 +116,7 @@ namespace wolf * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS! * * #### See also - * - IntrinsicsFactory: to create intrinsic structs deriving from IntrinsicsBase directly from YAML files. + * - ParamsSensorFactory: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files. * - ProcessorFactory: to create processors that will be bound to sensors. * - Problem::installSensor() : to install sensors in WOLF Problem. * @@ -124,13 +124,13 @@ namespace wolf * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp: * * \code - * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics) * { * // check extrinsics vector * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); * * // cast instrinsics to good type - * IntrinsicsCamera* intrinsics_ptr = (IntrinsicsCamera*) _intrinsics; + * ParamsSensorCamera* intrinsics_ptr = (ParamsSensorCamera*) _intrinsics; * * // Do create the SensorCamera object, and complete its setup * SensorCamera* sen_ptr = new SensorCamera(_extrinsics_pq, intrinsics_ptr); @@ -189,7 +189,7 @@ namespace wolf * // a pointer to the intrinsics struct: * * Eigen::VectorXd extrinsics_1(7); // give it some values... - * IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct + * ParamsSensorCamera intrinsics_1({...}); // see ParamsSensorFactory to fill in the derived struct * * SensorBasePtr camera_1_ptr = * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 ); @@ -197,7 +197,7 @@ namespace wolf * // A second camera... with a different name! * * Eigen::VectorXd extrinsics_2(7); - * IntrinsicsCamera intrinsics_2({...}); + * ParamsSensorCamera intrinsics_2({...}); * * SensorBasePtr camera_2_ptr = * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 ); @@ -209,20 +209,20 @@ namespace wolf * You can also check the code in the example file ````src/examples/test_wolf_factories.cpp````. */ -// Intrinsics factory -struct IntrinsicsBase; -typedef Factory<IntrinsicsBase, - const std::string&> IntrinsicsFactory; +// ParamsSensor factory +struct ParamsSensorBase; +typedef Factory<ParamsSensorBase, + const std::string&> ParamsSensorFactory; template<> -inline std::string IntrinsicsFactory::getClass() +inline std::string ParamsSensorFactory::getClass() { - return "IntrinsicsFactory"; + return "ParamsSensorFactory"; } // Sensor factory typedef Factory<SensorBase, const std::string&, - const Eigen::VectorXd&, const IntrinsicsBasePtr> SensorFactory; + const Eigen::VectorXd&, const ParamsSensorBasePtr> SensorFactory; template<> inline std::string SensorFactory::getClass() diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h index b27d1d768310ef1d14baabcfbe2a9cb13960a080..6f16e71fef41d774539464c2c001d719fd9d50e0 100644 --- a/include/core/sensor/sensor_odom_2D.h +++ b/include/core/sensor/sensor_odom_2D.h @@ -7,27 +7,27 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom2D); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom2D); -struct IntrinsicsOdom2D : public IntrinsicsBase +struct ParamsSensorOdom2D : public ParamsSensorBase { - virtual ~IntrinsicsOdom2D() = default; + virtual ~ParamsSensorOdom2D() = default; double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation - IntrinsicsOdom2D() + ParamsSensorOdom2D() { //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. } - IntrinsicsOdom2D(std::string _unique_name, const ParamsServer& _server): - IntrinsicsBase(_unique_name, _server) + ParamsSensorOdom2D(std::string _unique_name, const ParamsServer& _server): + ParamsSensorBase(_unique_name, _server) { k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp"); k_rot_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_rot_to_rot"); } std::string print() const { - return "\n" + IntrinsicsBase::print() + "\n" + return "\n" + ParamsSensorBase::print() + "\n" + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n" + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n"; } @@ -42,9 +42,9 @@ class SensorOdom2D : public SensorBase double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation public: - SensorOdom2D(Eigen::VectorXd _extrinsics, const IntrinsicsOdom2D& _intrinsics); - SensorOdom2D(Eigen::VectorXd _extrinsics, IntrinsicsOdom2DPtr _intrinsics); - WOLF_SENSOR_CREATE(SensorOdom2D, IntrinsicsOdom2D, 3); + SensorOdom2D(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2D& _intrinsics); + SensorOdom2D(Eigen::VectorXd _extrinsics, ParamsSensorOdom2DPtr _intrinsics); + WOLF_SENSOR_CREATE(SensorOdom2D, ParamsSensorOdom2D, 3); virtual ~SensorOdom2D(); diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h index cd7493b33e0f29fd55cfa409b019a2c705d0b5f6..b02fa3c38cfc3bdcc66063d264b67107984e5cf8 100644 --- a/include/core/sensor/sensor_odom_3D.h +++ b/include/core/sensor/sensor_odom_3D.h @@ -14,21 +14,21 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(IntrinsicsOdom3D); +WOLF_STRUCT_PTR_TYPEDEFS(ParamsSensorOdom3D); -struct IntrinsicsOdom3D : public IntrinsicsBase +struct ParamsSensorOdom3D : public ParamsSensorBase { double k_disp_to_disp; ///< ratio of displacement variance to displacement, for odometry noise calculation double k_disp_to_rot; ///< ratio of displacement variance to rotation, for odometry noise calculation double k_rot_to_rot; ///< ratio of rotation variance to rotation, for odometry noise calculation double min_disp_var; double min_rot_var; - IntrinsicsOdom3D() + ParamsSensorOdom3D() { //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE. } - IntrinsicsOdom3D(std::string _unique_name, const ParamsServer& _server): - IntrinsicsBase(_unique_name, _server) + ParamsSensorOdom3D(std::string _unique_name, const ParamsServer& _server): + ParamsSensorBase(_unique_name, _server) { k_disp_to_disp = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_disp"); k_disp_to_rot = _server.getParam<double>(prefix + _unique_name + "/k_disp_to_rot"); @@ -38,14 +38,14 @@ struct IntrinsicsOdom3D : public IntrinsicsBase } std::string print() const { - return "\n" + IntrinsicsBase::print() + "\n" + return "\n" + ParamsSensorBase::print() + "\n" + "k_disp_to_disp: " + std::to_string(k_disp_to_disp) + "\n" + "k_disp_to_rot: " + std::to_string(k_disp_to_rot) + "\n" + "k_rot_to_rot: " + std::to_string(k_rot_to_rot) + "\n" + "min_disp_var: " + std::to_string(min_disp_var) + "\n" + "min_rot_var: " + std::to_string(min_rot_var) + "\n"; } - virtual ~IntrinsicsOdom3D() = default; + virtual ~ParamsSensorOdom3D() = default; }; WOLF_PTR_TYPEDEFS(SensorOdom3D); @@ -60,9 +60,9 @@ class SensorOdom3D : public SensorBase double min_rot_var_; public: - SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsOdom3D& params); - SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, IntrinsicsOdom3DPtr params); - WOLF_SENSOR_CREATE(SensorOdom3D, IntrinsicsOdom3D, 7); + SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3D& params); + SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3DPtr params); + WOLF_SENSOR_CREATE(SensorOdom3D, ParamsSensorOdom3D, 7); virtual ~SensorOdom3D(); diff --git a/include/core/solver/solver_factory.h b/include/core/solver/solver_factory.h index 64d980c46a1a4a27b9af99feba7cd6952aeb0a55..40caeaf666c95f7369e5dd2e8328a46d22bef259 100644 --- a/include/core/solver/solver_factory.h +++ b/include/core/solver/solver_factory.h @@ -11,7 +11,7 @@ namespace wolf { class SensorBase; -struct IntrinsicsBase; +struct ParamsSensorBase; } // wolf @@ -75,11 +75,11 @@ namespace wolf * The method SensorCamera::create() exists in the SensorCamera class as a static method. * All these ````SensorXxx::create()```` methods need to have exactly the same API, regardless of the sensor type. * This API includes a sensor name, a vector of extrinsic parameters, - * and a pointer to a base struct of intrinsic parameters, IntrinsicsBasePtr, + * and a pointer to a base struct of intrinsic parameters, ParamsSensorBasePtr, * that can be derived for each derived sensor: * * \code - * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics) * \endcode * * See further down for an implementation example. @@ -116,7 +116,7 @@ namespace wolf * and to assign it the appropriate processors. DO NOT USE IT WITH DUMMY PARAMETERS! * * #### See also - * - IntrinsicsFactory: to create intrinsic structs deriving from IntrinsicsBase directly from YAML files. + * - ParamsSensorFactory: to create intrinsic structs deriving from ParamsSensorBase directly from YAML files. * - ProcessorFactory: to create processors that will be bound to sensors. * - Problem::installSensor() : to install sensors in WOLF Problem. * @@ -124,13 +124,13 @@ namespace wolf * Here is an example of SensorCamera::create() extracted from sensor_camera.cpp: * * \code - * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics) + * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, ParamsSensorBasePtr _intrinsics) * { * // check extrinsics vector * assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D."); * * // cast instrinsics to good type - * IntrinsicsCamera* intrinsics_ptr = (IntrinsicsCamera*) _intrinsics; + * ParamsSensorCamera* intrinsics_ptr = (ParamsSensorCamera*) _intrinsics; * * // Do create the SensorCamera object, and complete its setup * SensorCamera* sen_ptr = new SensorCamera(_extrinsics_pq, intrinsics_ptr); @@ -189,7 +189,7 @@ namespace wolf * // a pointer to the intrinsics struct: * * Eigen::VectorXd extrinsics_1(7); // give it some values... - * IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct + * ParamsSensorCamera intrinsics_1({...}); // see ParamsSensorFactory to fill in the derived struct * * SensorBasePtr camera_1_ptr = * SensorFactory::get().create ( "CAMERA" , "Front-left camera" , extrinsics_1 , &intrinsics_1 ); @@ -197,7 +197,7 @@ namespace wolf * // A second camera... with a different name! * * Eigen::VectorXd extrinsics_2(7); - * IntrinsicsCamera intrinsics_2({...}); + * ParamsSensorCamera intrinsics_2({...}); * * SensorBasePtr camera_2_ptr = * SensorFactory::get().create( "CAMERA" , "Front-right camera" , extrinsics_2 , &intrinsics_2 ); diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp index 7abc062844627db0f38d356e2ccd64b0477aa295..c25761dc9c73a56d9d7e0e728c0d7e794844aa8d 100644 --- a/src/problem/problem.cpp +++ b/src/problem/problem.cpp @@ -174,7 +174,7 @@ Problem::~Problem() SensorBasePtr Problem::installSensor(const std::string& _sen_type, // const std::string& _unique_sensor_name, // const Eigen::VectorXd& _extrinsics, // - IntrinsicsBasePtr _intrinsics) + ParamsSensorBasePtr _intrinsics) { SensorBasePtr sen_ptr = SensorFactory::get().create(_sen_type, _unique_sensor_name, _extrinsics, _intrinsics); sen_ptr->link(getHardware()); @@ -190,11 +190,11 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, // if (_intrinsics_filename != "") { assert(file_exists(_intrinsics_filename) && "Cannot install sensor: intrinsics' YAML file does not exist."); - IntrinsicsBasePtr intr_ptr = IntrinsicsFactory::get().create(_sen_type, _intrinsics_filename); + ParamsSensorBasePtr intr_ptr = ParamsSensorFactory::get().create(_sen_type, _intrinsics_filename); return installSensor(_sen_type, _unique_sensor_name, _extrinsics, intr_ptr); } else - return installSensor(_sen_type, _unique_sensor_name, _extrinsics, IntrinsicsBasePtr()); + return installSensor(_sen_type, _unique_sensor_name, _extrinsics, ParamsSensorBasePtr()); } diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp index 5b85000eca793039df8dc5b09b327f951440b0d7..ae48fcceb261da67fa2b940aa06248b3d5006e48 100644 --- a/src/sensor/sensor_diff_drive.cpp +++ b/src/sensor/sensor_diff_drive.cpp @@ -12,7 +12,7 @@ namespace wolf { SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics, - IntrinsicsDiffDrivePtr _intrinsics) : + ParamsSensorDiffDrivePtr _intrinsics) : SensorBase("SensorDiffDrive", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp index 89154e6144f7c244f4e98090ba70ee5b02de244f..626b96a86c3957cc8f9dcacf536152d1f28376dd 100644 --- a/src/sensor/sensor_odom_2D.cpp +++ b/src/sensor/sensor_odom_2D.cpp @@ -4,7 +4,7 @@ namespace wolf { -SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, const IntrinsicsOdom2D& _intrinsics) : +SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, const ParamsSensorOdom2D& _intrinsics) : SensorBase("SensorOdom2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2), k_disp_to_disp_(_intrinsics.k_disp_to_disp), k_rot_to_rot_(_intrinsics.k_rot_to_rot) @@ -13,7 +13,7 @@ SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, const IntrinsicsOdom2D& // } -SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, IntrinsicsOdom2DPtr _intrinsics) : +SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, ParamsSensorOdom2DPtr _intrinsics) : SensorOdom2D(_extrinsics, *_intrinsics) { // diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp index 9f867388533c726647f9bc073479e2d1e1b24101..181e9326631c21409b35dd0232773576aca5c72c 100644 --- a/src/sensor/sensor_odom_3D.cpp +++ b/src/sensor/sensor_odom_3D.cpp @@ -12,7 +12,7 @@ namespace wolf { -SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) : +SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const ParamsSensorOdom3D& _intrinsics) : SensorBase("SensorOdom3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6), k_disp_to_disp_(_intrinsics.k_disp_to_disp), k_disp_to_rot_(_intrinsics.k_disp_to_rot), @@ -26,7 +26,7 @@ SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const Intrinsi setNoiseCov(noise_cov_); // sets also noise_std_ } -SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) : +SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, ParamsSensorOdom3DPtr _intrinsics) : SensorOdom3D(_extrinsics_pq, *_intrinsics) { // diff --git a/src/yaml/sensor_odom_2D_yaml.cpp b/src/yaml/sensor_odom_2D_yaml.cpp index 8d05e662673336f9b09bf5dbde0d751e6e11e0bc..f157c221b223ecd813613b12c3f9460980af264a 100644 --- a/src/yaml/sensor_odom_2D_yaml.cpp +++ b/src/yaml/sensor_odom_2D_yaml.cpp @@ -21,13 +21,13 @@ namespace wolf namespace { -static IntrinsicsBasePtr createIntrinsicsOdom2D(const std::string & _filename_dot_yaml) +static ParamsSensorBasePtr createParamsSensorOdom2D(const std::string & _filename_dot_yaml) { YAML::Node config = YAML::LoadFile(_filename_dot_yaml); if (config["type"].as<std::string>() == "SensorOdom2D") { - auto params = std::make_shared<IntrinsicsOdom2D>(); + auto params = std::make_shared<ParamsSensorOdom2D>(); params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>(); params->k_rot_to_rot = config["k_rot_to_rot"] .as<double>(); @@ -40,7 +40,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom2D(const std::string & _filename_do } // Register in the SensorFactory -const bool WOLF_UNUSED registered_odom_2D_intr = IntrinsicsFactory::get().registerCreator("SensorOdom2D", createIntrinsicsOdom2D); +const bool WOLF_UNUSED registered_odom_2D_intr = ParamsSensorFactory::get().registerCreator("SensorOdom2D", createParamsSensorOdom2D); } // namespace [unnamed] diff --git a/src/yaml/sensor_odom_3D_yaml.cpp b/src/yaml/sensor_odom_3D_yaml.cpp index 4af4efbc605e298337066a72ee056cad3a52f1f6..61abfae1056ff03eed1421ef3221062c2c875281 100644 --- a/src/yaml/sensor_odom_3D_yaml.cpp +++ b/src/yaml/sensor_odom_3D_yaml.cpp @@ -20,13 +20,13 @@ namespace wolf namespace { -static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_dot_yaml) +static ParamsSensorBasePtr createParamsSensorOdom3D(const std::string & _filename_dot_yaml) { YAML::Node config = YAML::LoadFile(_filename_dot_yaml); if (config["type"].as<std::string>() == "SensorOdom3D") { - IntrinsicsOdom3DPtr params = std::make_shared<IntrinsicsOdom3D>(); + ParamsSensorOdom3DPtr params = std::make_shared<ParamsSensorOdom3D>(); params->k_disp_to_disp = config["k_disp_to_disp"] .as<double>(); params->k_disp_to_rot = config["k_disp_to_rot"] .as<double>(); @@ -42,7 +42,7 @@ static IntrinsicsBasePtr createIntrinsicsOdom3D(const std::string & _filename_do } // Register in the SensorFactory -const bool WOLF_UNUSED registered_odom_3D_intr = IntrinsicsFactory::get().registerCreator("SensorOdom3D", createIntrinsicsOdom3D); +const bool WOLF_UNUSED registered_odom_3D_intr = ParamsSensorFactory::get().registerCreator("SensorOdom3D", createParamsSensorOdom3D); } // namespace [unnamed] diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp index 93bcb43444f2ad303b5e544cf482ecbad7195fad..e2def5779a6d2dab385df5cbfb599fbd3c7a944c 100644 --- a/test/gtest_emplace.cpp +++ b/test/gtest_emplace.cpp @@ -120,7 +120,7 @@ TEST(Emplace, EmplaceDerived) ProblemPtr P = Problem::create("POV", 3); auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true)); - auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXd(3), IntrinsicsOdom2D()); + auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXd(3), ParamsSensorOdom2D()); auto cov = Eigen::MatrixXd::Identity(2,2); auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr); auto m = Eigen::Matrix<double,9,6>(); diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp index f9c9212921ae9b780fc17c463beaef43967a0e0a..d03a9d3a5c728682d0cdef0b609ebb52edeff394 100644 --- a/test/gtest_factor_autodiff.cpp +++ b/test/gtest_factor_autodiff.cpp @@ -23,7 +23,7 @@ TEST(CaptureAutodiff, ConstructorOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1))); // SENSOR - IntrinsicsOdom2D intrinsics_odo; + ParamsSensorOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 0.1; intrinsics_odo.k_rot_to_rot = 0.1; SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo); @@ -54,7 +54,7 @@ TEST(CaptureAutodiff, ResidualOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - IntrinsicsOdom2D intrinsics_odo; + ParamsSensorOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 0.1; intrinsics_odo.k_rot_to_rot = 0.1; SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo); @@ -98,7 +98,7 @@ TEST(CaptureAutodiff, JacobianOdom2d) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - IntrinsicsOdom2D intrinsics_odo; + ParamsSensorOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 0.1; intrinsics_odo.k_rot_to_rot = 0.1; SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo); @@ -176,7 +176,7 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic) FrameBasePtr fr2_ptr(std::make_shared<FrameBase>(TimeStamp(0), std::make_shared<StateBlock>(f2_pose.head<2>()), std::make_shared<StateBlock>(f2_pose.tail<1>()))); // SENSOR - IntrinsicsOdom2D intrinsics_odo; + ParamsSensorOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 0.1; intrinsics_odo.k_rot_to_rot = 0.1; SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo); diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp index e92ae4b558a34f1019c493d2a5c2bba6685f4d14..67ef0974061dfa88339bbf3be362794ec6af060d 100644 --- a/test/gtest_factor_diff_drive.cpp +++ b/test/gtest_factor_diff_drive.cpp @@ -119,7 +119,7 @@ class FactorDiffDriveTest : public testing::Test ProblemPtr problem; CeresManagerPtr solver; TrajectoryBasePtr trajectory; - IntrinsicsDiffDrivePtr intr; + ParamsSensorDiffDrivePtr intr; SensorDiffDrivePtr sensor; ProcessorParamsDiffDrivePtr params; ProcessorDiffDrivePublicPtr processor; @@ -143,7 +143,7 @@ class FactorDiffDriveTest : public testing::Test solver = std::make_shared<CeresManager>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! - intr = std::make_shared<IntrinsicsDiffDrive>(); + intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!! @@ -420,7 +420,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt) CeresManagerPtr solver = std::make_shared<CeresManager>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! - IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>(); + ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!! @@ -552,7 +552,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics) CeresManagerPtr solver = std::make_shared<CeresManager>(problem); // make a sensor first // DO NOT MODIFY THESE VALUES !!! - IntrinsicsDiffDrivePtr intr = std::make_shared<IntrinsicsDiffDrive>(); + ParamsSensorDiffDrivePtr intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 0.95; intr->radius_right = 0.98; intr->wheel_separation = 1.05; diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp index 10dcda6825250e8f949a7b43391ce2d3676f6f82..e37ef4935c3deb1e2c1e4449a6b7a294ea9a000a 100644 --- a/test/gtest_frame_base.cpp +++ b/test/gtest_frame_base.cpp @@ -53,7 +53,7 @@ TEST(FrameBase, LinksBasic) ASSERT_FALSE(F->getProblem()); // ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert() // ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected - SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3d::Zero(), IntrinsicsOdom2D()); + SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3d::Zero(), ParamsSensorOdom2D()); ASSERT_FALSE(F->getCaptureOf(S)); ASSERT_TRUE(F->getCaptureList().empty()); ASSERT_TRUE(F->getConstrainedByList().empty()); @@ -65,7 +65,7 @@ TEST(FrameBase, LinksToTree) // Problem with 2 frames and one motion factor between them ProblemPtr P = Problem::create("PO", 2); TrajectoryBasePtr T = P->getTrajectory(); - IntrinsicsOdom2D intrinsics_odo; + ParamsSensorOdom2D intrinsics_odo; intrinsics_odo.k_disp_to_disp = 1; intrinsics_odo.k_rot_to_rot = 1; auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3d::Zero(), intrinsics_odo); diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp index d7ec37dd0bf31a65ef2c4b163d9969df4764f4bf..2c5bcbf1852a48baff40d2cf2f9464aa9157f906 100644 --- a/test/gtest_param_prior.cpp +++ b/test/gtest_param_prior.cpp @@ -22,7 +22,7 @@ Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).fi Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished()); Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished()); -SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("SensorOdom3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>())); +SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("SensorOdom3D", "odometer", initial_extrinsics, std::make_shared<ParamsSensorOdom3D>())); TEST(ParameterPrior, initial_extrinsics) { diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp index d39ce95f7259b1822c4af0b9d7cca5eb0b9903a6..1b74256d81808822dd1de31eed99e83098511c4d 100644 --- a/test/gtest_problem.cpp +++ b/test/gtest_problem.cpp @@ -92,7 +92,7 @@ TEST(Problem, Processor) ASSERT_FALSE(P->getProcessorMotion()); // add a motion sensor and processor - auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); + auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), ParamsSensorOdom3D()); // add motion processor auto Pm = ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>()); diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp index 330326d9a677a844616af9ed930439df9d8dee84..612f66019cd62b892a300df15da4a809b0d51e9a 100644 --- a/test/gtest_processor_diff_drive.cpp +++ b/test/gtest_processor_diff_drive.cpp @@ -109,7 +109,7 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive class ProcessorDiffDriveTest : public testing::Test { public: - IntrinsicsDiffDrivePtr intr; + ParamsSensorDiffDrivePtr intr; SensorDiffDrivePtr sensor; ProcessorParamsDiffDrivePtr params; ProcessorDiffDrivePublicPtr processor; @@ -120,7 +120,7 @@ class ProcessorDiffDriveTest : public testing::Test problem = Problem::create("PO", 2); // make a sensor first // DO NOT MODIFY THESE VALUES !!! - intr = std::make_shared<IntrinsicsDiffDrive>(); + intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!! intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!! @@ -159,7 +159,7 @@ TEST(ProcessorDiffDrive, constructor) TEST(ProcessorDiffDrive, create) { // make a sensor first - auto intr = std::make_shared<IntrinsicsDiffDrive>(); + auto intr = std::make_shared<ParamsSensorDiffDrive>(); Vector3d extr(1,2,3); auto sen = std::make_shared<SensorDiffDrive>(extr, intr); diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp index 06e20263bbdde76466944039467fd1e552580e94..8e1b0d8c7f6c6a16fbe4648dc035ee65c6c845b5 100644 --- a/test/gtest_processor_tracker_feature_dummy.cpp +++ b/test/gtest_processor_tracker_feature_dummy.cpp @@ -82,7 +82,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test problem = Problem::create("PO", 2); // Install camera - sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>()); + sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>()); // Install processor params = std::make_shared<ProcessorParamsTrackerFeatureDummy>(); diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp index 72b7f55d8dde87e39c8bb161ea0a768b79e2ba00..b397a512415c6643985c57ac551bceb6e5400994 100644 --- a/test/gtest_processor_tracker_landmark_dummy.cpp +++ b/test/gtest_processor_tracker_landmark_dummy.cpp @@ -100,7 +100,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test problem = Problem::create("PO", 2); // Install camera - sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>()); + sensor = problem->installSensor("SensorOdom2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<ParamsSensorBase>()); // Install processor params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>(); diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp index 3e5d696581c0c5700ff5e43bd837ba634e68b287..e2936a8ddec8c06a6fc28df0f8c7dc5ed1c90f97 100644 --- a/test/gtest_sensor_diff_drive.cpp +++ b/test/gtest_sensor_diff_drive.cpp @@ -16,7 +16,7 @@ using namespace Eigen; TEST(DiffDrive, constructor) { - auto intr = std::make_shared<IntrinsicsDiffDrive>(); + auto intr = std::make_shared<ParamsSensorDiffDrive>(); Vector3d extr(1,2,3); auto sen = std::make_shared<SensorDiffDrive>(extr, intr); @@ -29,7 +29,7 @@ TEST(DiffDrive, constructor) TEST(DiffDrive, getParams) { - auto intr = std::make_shared<IntrinsicsDiffDrive>(); + auto intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 2; intr->radius_right = 3; intr->ticks_per_wheel_revolution = 4; @@ -50,7 +50,7 @@ TEST(DiffDrive, getParams) TEST(DiffDrive, create) { - auto intr = std::make_shared<IntrinsicsDiffDrive>(); + auto intr = std::make_shared<ParamsSensorDiffDrive>(); intr->radius_left = 2; intr->radius_right = 3; intr->ticks_per_wheel_revolution = 4;