diff --git a/src/sensor_base.cpp b/src/sensor_base.cpp index 8ebe7bc40edfa84212d80720387393a8dd6df258..7862208791bea5f7cd86d3594da70b3458e60424 100644 --- a/src/sensor_base.cpp +++ b/src/sensor_base.cpp @@ -6,14 +6,20 @@ namespace wolf { unsigned int SensorBase::sensor_id_count_ = 0; -SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, - const unsigned int _noise_size, const bool _extr_dyn) : +SensorBase::SensorBase(const std::string& _type, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr, + const unsigned int _noise_size, + const bool _extr_dyn, + const bool _intr_dyn) : NodeBase("SENSOR", _type), hardware_ptr_(), state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed. is_removing_(false), sensor_id_(++sensor_id_count_), // simple ID factory extrinsic_dynamic_(_extr_dyn), + intrinsic_dynamic_(_intr_dyn), noise_std_(_noise_size), noise_cov_(_noise_size, _noise_size) { @@ -24,14 +30,20 @@ SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBloc // } -SensorBase::SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, - const Eigen::VectorXs & _noise_std, const bool _extr_dyn) : +SensorBase::SensorBase(const std::string& _type, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr, + const Eigen::VectorXs & _noise_std, + const bool _extr_dyn, + const bool _intr_dyn) : NodeBase("SENSOR", _type), hardware_ptr_(), state_block_vec_(6), // allow for 3 state blocks by default. Resize in derived constructors if needed. is_removing_(false), sensor_id_(++sensor_id_count_), // simple ID factory extrinsic_dynamic_(_extr_dyn), + intrinsic_dynamic_(_intr_dyn), noise_std_(_noise_std), noise_cov_(_noise_std.size(), _noise_std.size()) { diff --git a/src/sensor_base.h b/src/sensor_base.h index 71c38883217b46077f894a3d4b00d2c4ea459f45..9b8bd4e6fbd02a545b66fc4868cdc85ed995465f 100644 --- a/src/sensor_base.h +++ b/src/sensor_base.h @@ -41,6 +41,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa unsigned int sensor_id_; // sensor ID bool extrinsic_dynamic_;// extrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented. + bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes. TODO: Not Yet Implemented. Eigen::VectorXs noise_std_; // std of sensor noise Eigen::MatrixXs noise_cov_; // cov matrix of noise @@ -57,7 +58,13 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving) * **/ - SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const unsigned int _noise_size, const bool _extr_dyn = false); + SensorBase(const std::string& _type, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr, + const unsigned int _noise_size, + const bool _extr_dyn = false, + const bool _intr_dyn = false); /** \brief Constructor with noise std vector * @@ -70,7 +77,14 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa * \param _extr_dyn Flag indicating if extrinsics are dynamic (moving) or static (not moving) * **/ - SensorBase(const std::string& _type, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _intr_ptr, const Eigen::VectorXs & _noise_std, const bool _extr_dyn = false); + SensorBase(const std::string& _type, + StateBlockPtr _p_ptr, + StateBlockPtr _o_ptr, + StateBlockPtr _intr_ptr, + const Eigen::VectorXs & _noise_std, + const bool _extr_dyn = false, + const bool _intr_dyn = false); + virtual ~SensorBase(); void remove();