diff --git a/include/vision/sensor/sensor_camera.h b/include/vision/sensor/sensor_camera.h index 7ee6529d355355928a2d439602b35dbffceacd59..4af4641f6691636955f339f77fb300952771c45b 100644 --- a/include/vision/sensor/sensor_camera.h +++ b/include/vision/sensor/sensor_camera.h @@ -25,11 +25,11 @@ struct ParamsSensorCamera : public ParamsSensorBase ParamsSensorCamera(std::string _unique_name, const ParamsServer& _server): ParamsSensorBase(_unique_name, _server) { - width = _server.getParam<unsigned int>(_unique_name + "/width"); - height = _server.getParam<unsigned int>(_unique_name + "/height"); - pinhole_model_raw = _server.getParam<Eigen::Vector4d>(_unique_name + "/pinhole_model_raw"); - pinhole_model_rectified = _server.getParam<Eigen::Vector4d>(_unique_name + "/pinhole_model_rectified"); - distortion = _server.getParam<Eigen::VectorXd>(_unique_name + "/distortion"); + width = _server.getParam<unsigned int>(prefix + _unique_name + "/width"); + height = _server.getParam<unsigned int>(prefix + _unique_name + "/height"); + pinhole_model_raw = _server.getParam<Eigen::Vector4d>(prefix + _unique_name + "/pinhole_model_raw"); + pinhole_model_rectified = _server.getParam<Eigen::Vector4d>(prefix + _unique_name + "/pinhole_model_rectified"); + distortion = _server.getParam<Eigen::VectorXd>(prefix + _unique_name + "/distortion"); } std::string print() const { @@ -52,6 +52,7 @@ class SensorCamera : public SensorBase SensorCamera(const Eigen::VectorXd & _extrinsics, const ParamsSensorCamera& _intrinsics); SensorCamera(const Eigen::VectorXd & _extrinsics, ParamsSensorCameraPtr _intrinsics_ptr); + WOLF_SENSOR_CREATE(SensorCamera, ParamsSensorCamera, 7); ~SensorCamera() override; @@ -82,11 +83,6 @@ class SensorCamera : public SensorBase public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html) - - static SensorBasePtr create(const std::string & _unique_name, // - const Eigen::VectorXd& _extrinsics, // - const ParamsSensorBasePtr _intrinsics); - }; inline bool SensorCamera::useRawImages() diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp index 28195349466845b115c884a9311c1055f5807ed5..a236256e6334eac2f580957acafc891688c948ef 100644 --- a/src/sensor/sensor_camera.cpp +++ b/src/sensor/sensor_camera.cpp @@ -46,21 +46,6 @@ Eigen::Matrix3d SensorCamera::setIntrinsicMatrix(Eigen::Vector4d _pinhole_model) K.row(2) << 0, 0, 1; return K; } - -// Define the factory method -SensorBasePtr SensorCamera::create(const std::string& _unique_name, // - const Eigen::VectorXd& _extrinsics_pq, // - const ParamsSensorBasePtr _intrinsics) -{ - assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d."); - - std::shared_ptr<ParamsSensorCamera> intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics); - SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr); - sen_ptr->setName(_unique_name); - - return sen_ptr; -} - } // namespace wolf // Register in the FactorySensor @@ -68,5 +53,6 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, // namespace wolf { WOLF_REGISTER_SENSOR(SensorCamera) +WOLF_REGISTER_SENSOR_AUTO(SensorCamera) } // namespace wolf