diff --git a/src/processor/processor_imu.cpp b/src/processor/processor_imu.cpp index 4b9009461b39d5dc8254725e08dcad37a6a966ae..f1b7ffbf2e5185526be6b0056c503681b7afa8f4 100644 --- a/src/processor/processor_imu.cpp +++ b/src/processor/processor_imu.cpp @@ -102,6 +102,11 @@ void ProcessorImu::setCalibration (const CaptureBasePtr _capture, const VectorXd FactorBasePtr ProcessorImu::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) { + assert(_capture_origin != nullptr && _capture_origin->getType() == "CaptureImu" && "ProcessorImu::emplaceFactor: capture is null or not of type CaptureImu"); + assert(_feature_motion != nullptr && _feature_motion->getType() == "FeatureImu" && "ProcessorImu::emplaceFactor: feature is null or not of type FeatureImu"); + assert(_capture_origin->getFrame() && "ProcessorImu::emplaceFactor: capture not linked to a frame"); + assert(_feature_motion->getCapture() and _feature_motion->getCapture()->getFrame() && "ProcessorImu::emplaceFactor: feature not linked to a capture or a frame"); + CaptureImuPtr cap_imu = std::static_pointer_cast<CaptureImu>(_capture_origin); FeatureImuPtr ftr_imu = std::static_pointer_cast<FeatureImu>(_feature_motion);