diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h index 200f00badcd386b2ea2d6945f6d244e71b993b1a..71a152b098ab1f2654be2ea53e13b794c614d08b 100644 --- a/include/laser/sensor/sensor_laser_2D.h +++ b/include/laser/sensor/sensor_laser_2D.h @@ -56,6 +56,7 @@ class SensorLaser2D : public SensorBase SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params); SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params); SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params); + SensorLaser2D(const Eigen::VectorXs& _extrinsics, IntrinsicsLaser2DPtr _params); virtual ~SensorLaser2D(); diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp index 05b55a7e28e3df57cc6da901e3b4158adcd1ba1b..bff07ba6ede76c6e5cd0ac762877596bd2bd2f3f 100644 --- a/src/sensor/sensor_laser_2D.cpp +++ b/src/sensor/sensor_laser_2D.cpp @@ -1,5 +1,6 @@ #include "laser/sensor/sensor_laser_2D.h" #include "core/state_block/state_block.h" +#include "core/state_block/state_angle.h" namespace wolf { @@ -36,6 +37,13 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, Intrins // } +SensorLaser2D::SensorLaser2D(const Eigen::VectorXs& _extrinsics, IntrinsicsLaser2DPtr _params) : + SensorLaser2D(std::make_shared<StateBlock>(_extrinsics.head(2), true, nullptr), + std::make_shared<StateAngle>(_extrinsics(2), true), + _params) +{ +} + SensorLaser2D::~SensorLaser2D() { // @@ -103,9 +111,6 @@ SensorBasePtr SensorLaser2D::createAutoConf(const std::string& _unique_name, con //#include "intrinsics_factory.h" namespace wolf { WOLF_REGISTER_SENSOR("LASER 2D", SensorLaser2D) -//const bool registered_laser_params = IntrinsicsFactory::get().registerCreator("LASER 2D", createIntrinsicsLaser2D); -} // namespace wolf -#include "core/sensor/autoconf_sensor_factory.h" -namespace wolf { WOLF_REGISTER_SENSOR_AUTO("LASER 2D", SensorLaser2D) } // namespace wolf +