Fix state initialization odom icp

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();
 }