From 60ca451e904bab3f9e611a8cd07b65fc2a56337e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Mon, 26 Aug 2019 13:26:03 +0200
Subject: [PATCH] Resize provided vector ref to appropriate size
also assert origin_ptr_
---
include/core/processor/processor_motion.h | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 9bc5027fd..d8a1361ac 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -515,6 +515,13 @@ inline Eigen::VectorXs ProcessorMotion::getCurrentState()
inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
{
+ // ensure integrity
+ assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
+
+ // ensure proper size of the provided reference
+ _x.resize( getProblem()->getFrameStructureSize() );
+
+ // do get timestamp and state corrected by possible self-calibrations
Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
--
GitLab