Skip to content
Snippets Groups Projects
Commit 58c545a1 authored by Médéric Fourmy's avatar Médéric Fourmy
Browse files

Fixed a state size related bug

parent 8e1218d9
No related branches found
No related tags found
1 merge request!339Adapting to multiple processor motion 285
...@@ -374,7 +374,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const ...@@ -374,7 +374,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step); VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
// ensure proper size of the provided reference // 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 // 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_; double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
...@@ -383,7 +383,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const ...@@ -383,7 +383,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
else else
{ {
// We could not find any CaptureMotion for the time stamp requested // 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?") WOLF_TRACE("Did you forget to call Problem::setPrior() in your application?")
return false; return false;
} }
...@@ -567,7 +567,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp ...@@ -567,7 +567,8 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
++frame_rev_iter) ++frame_rev_iter)
{ {
frame = *frame_rev_iter; frame = *frame_rev_iter;
capture = frame->getCaptureOf(getSensor()); auto sensor = getSensor();
capture = frame->getCaptureOf(sensor);
if (capture != nullptr) if (capture != nullptr)
{ {
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion // Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment