diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h index 0d52eb63a81c0ca963fbe6f8eb0a109e3a92d8c1..6ea4595f22496ccbd2f80b8c476951eb144bae9f 100644 --- a/include/laser/sensor/sensor_laser_2D.h +++ b/include/laser/sensor/sensor_laser_2D.h @@ -82,7 +82,8 @@ 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(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2DPtr _params); + SensorLaser2D(const Eigen::VectorXs& _extrinsics, const IntrinsicsLaser2DPtr _params); virtual ~SensorLaser2D(); diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp index 05b55a7e28e3df57cc6da901e3b4158adcd1ba1b..9c2918c68cb8d94fbd9458d51464ceb42319ba65 100644 --- a/src/sensor/sensor_laser_2D.cpp +++ b/src/sensor/sensor_laser_2D.cpp @@ -30,8 +30,20 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const I // } -SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params) : - SensorLaser2D(_p_ptr, _o_ptr, *_params) +SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2DPtr _params) : + SensorBase("LASER 2D", _p_ptr, _o_ptr, nullptr, 8), + scan_params_(_params->scan_params) +{ + // +} + +SensorLaser2D::SensorLaser2D(const Eigen::VectorXs& _extrinsics, const IntrinsicsLaser2DPtr _params) : + SensorBase("LASER 2D", + std::make_shared<StateBlock>(_extrinsics.head(2), false), + std::make_shared<StateAngle>(_extrinsics(2), false), + nullptr, + 8), + scan_params_(_params->scan_params) { // }