diff --git a/CMakeLists.txt b/CMakeLists.txt index e01bf02de0b028e1e1e5459b1874600f31244768..94c02b1fdf24a08738c6d72ee95cbd2a965ee5de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -265,6 +265,7 @@ SET(HDRS_SENSOR include/core/sensor/sensor_base.h include/core/sensor/sensor_diff_drive.h include/core/sensor/factory_sensor.h + include/core/sensor/sensor_model.h include/core/sensor/sensor_odom_2d.h include/core/sensor/sensor_odom_3d.h include/core/sensor/sensor_pose.h @@ -362,6 +363,7 @@ SET(SRCS_PROCESSOR SET(SRCS_SENSOR src/sensor/sensor_base.cpp src/sensor/sensor_diff_drive.cpp + src/sensor/sensor_model.cpp src/sensor/sensor_odom_2d.cpp src/sensor/sensor_odom_3d.cpp src/sensor/sensor_pose.cpp diff --git a/include/core/sensor/sensor_model.h b/include/core/sensor/sensor_model.h new file mode 100644 index 0000000000000000000000000000000000000000..5e2a37df70243f7d331dadde464fffd1d7f081ef --- /dev/null +++ b/include/core/sensor/sensor_model.h @@ -0,0 +1,38 @@ +#ifndef SRC_SENSOR_MODEL_H_ +#define SRC_SENSOR_MODEL_H_ + +//wolf includes +#include "core/sensor/sensor_base.h" + +namespace wolf { + +WOLF_PTR_TYPEDEFS(SensorModel); + +class SensorModel : public SensorBase +{ + public: + SensorModel(); + ~SensorModel() override; + + static SensorBasePtr create(const std::string& _unique_name, + const ParamsServer& _server) + { + auto sensor = std::make_shared<SensorModel>(); + sensor ->setName(_unique_name); + return sensor; + } + + static SensorBasePtr create(const std::string& _unique_name, + const Eigen::VectorXd& _extrinsics, + const ParamsSensorBasePtr _intrinsics) + { + auto sensor = std::make_shared<SensorModel>(); + sensor ->setName(_unique_name); + return sensor; + } +}; + + +} /* namespace wolf */ + +#endif /* SRC_SENSOR_POSE_H_ */ diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp index 16a535dc987deea66756f4c8468ac3005f2b3573..b87e4fb10c37d79ede036e72983faea5d7d12b7c 100644 --- a/src/capture/capture_motion.cpp +++ b/src/capture/capture_motion.cpp @@ -49,7 +49,7 @@ CaptureMotion::~CaptureMotion() bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance) { - assert(_ts.ok()); + assert(_ts.ok() and this->time_stamp_.ok()); // the same capture is within tolerance if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance) diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp index aac84686825ac6eb9e57c0bda3a1de62c2f8c622..95233cf96e55c29910aa8ac13168e220b3cde136 100644 --- a/src/frame/frame_base.cpp +++ b/src/frame/frame_base.cpp @@ -165,6 +165,7 @@ FrameBasePtr FrameBase::getNextFrame() const CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr) { + WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id()); capture_list_.push_back(_capt_ptr); return _capt_ptr; } diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp index 7af87c9ac5bc1ab2e81b8e8259ef140d9f7695a0..9b7f8da44e1e58c86277a2cb689401426932c723 100644 --- a/src/processor/processor_motion.cpp +++ b/src/processor/processor_motion.cpp @@ -896,6 +896,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp capture = frame->getCaptureOf(sensor); if (capture != nullptr) { + assert(std::dynamic_pointer_cast<CaptureMotion>(capture) != nullptr); // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion capture_motion = std::static_pointer_cast<CaptureMotion>(capture); diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp new file mode 100644 index 0000000000000000000000000000000000000000..6eb00f021667436360821f27e5684a512eb425e4 --- /dev/null +++ b/src/sensor/sensor_model.cpp @@ -0,0 +1,24 @@ +#include "core/sensor/sensor_model.h" + +namespace wolf { + + +SensorModel::SensorModel() : + SensorBase("SensorModel", nullptr, nullptr, nullptr, 6) +{ + // +} + +SensorModel::~SensorModel() +{ + // +} + +} // namespace wolf + +// Register in the FactorySensor +#include "core/sensor/factory_sensor.h" +namespace wolf { +WOLF_REGISTER_SENSOR(SensorModel); +WOLF_REGISTER_SENSOR_AUTO(SensorModel); +}