diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp index a362d6e7ad3bcc24deffff288cff3b25908d1a97..c7c467ada01838e47088ca5c6e3f38ea75923b42 100644 --- a/src/processor/processor_odom_icp.cpp +++ b/src/processor/processor_odom_icp.cpp @@ -277,6 +277,16 @@ FactorBasePtr ProcessorOdomIcp::emplaceFactor(FeatureBasePtr _feature) VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) const { + if (origin_ptr_ == nullptr or + origin_ptr_->isRemoving() or + last_ptr_ == nullptr or + last_ptr_->getFrame() == nullptr) // We do not have any info of where to find a valid state + // Further checking here for origin_ptr is redundant: if last=null, then origin=null too. + { + WOLF_DEBUG("Processor has no state. Returning an empty VectorComposite with no blocks"); + return VectorComposite(); // return empty state + } + auto w_T_ro = laser::trf2isometry(origin_ptr_->getFrame()->getP()->getState(), origin_ptr_->getFrame()->getO()->getState()); auto ro_T_so = laser::trf2isometry(origin_ptr_->getSensorP()->getState(), origin_ptr_->getSensorO()->getState()); const auto& rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead @@ -295,7 +305,7 @@ VectorComposite ProcessorOdomIcp::getState( const StateStructure& _structure ) c TimeStamp ProcessorOdomIcp::getTimeStamp() const { if( last_ptr_ == nullptr ) - return 0; + return TimeStamp::Invalid(); else return last_ptr_->getTimeStamp(); }