From 03182efe83191ea6abbe8a8ec5dff577b4a235e7 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 22 Jun 2020 12:35:29 +0200
Subject: [PATCH] Fix state initialization odom icp
---
src/processor/processor_odom_icp.cpp | 12 +++++++++++-
1 file changed, 11 insertions(+), 1 deletion(-)
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index a362d6e7a..c7c467ada 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();
}
--
GitLab