From 58c545a1a87160e8209695509f87fcf4247c3de2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Wed, 25 Mar 2020 04:03:04 +0100
Subject: [PATCH] Fixed a state size related bug
---
src/processor/processor_motion.cpp | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 08cd35ab8..9a0cbb90d 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -374,7 +374,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
// ensure proper size of the provided reference
- _x.resize( getProblem()->getFrameStructureSize() );
+ _x.resize( state_0.size() );
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
@@ -383,7 +383,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
else
{
// We could not find any CaptureMotion for the time stamp requested
- WOLF_ERROR("Could not find any Capture for the time stamp requested. ");
+ WOLF_ERROR("Could not find any Capture for the time stamp requested. ", _ts);
WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
return false;
}
@@ -567,7 +567,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensor());
+ auto sensor = getSensor();
+ capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
--
GitLab