From 76c3989c4b8fa601929dce7ff6618b2d5bd9206b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Wed, 18 Mar 2020 23:40:22 +0100
Subject: [PATCH] Fixed all gtests
---
src/problem/problem.cpp | 26 +++++++++++++++-----------
src/processor/processor_motion.cpp | 3 ++-
2 files changed, 17 insertions(+), 12 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 95d300f97..02afc83ee 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -375,15 +375,17 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
int idx = 0;
for (char sb_name: proc->getStateStructure()){
- if (states_to_concat_map.find(sb_name) != states_to_concat_map.end()){
- // not already there
+ // not already there
+ if (states_to_concat_map.find(sb_name) == states_to_concat_map.end()){
if (sb_name == 'O'){
- states_to_concat_map[sb_name] = proc_state.segment<4>(idx);
- idx += 4;
+ int size_sb = dim_ == 3 ? 4 : 1; // really bad...
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
}
else{
- states_to_concat_map[sb_name] = proc_state.segment<3>(idx);
- idx += 3;
+ int size_sb = dim_ == 3 ? 3 : 2;
+ states_to_concat_map[sb_name] = proc_state.segment(idx, size_sb);
+ idx += size_sb;
}
}
}
@@ -393,18 +395,20 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
for (auto state_map_it: states_to_concat_map){
concat_size += state_map_it.second.size();
}
- assert(concat_size == state_size_ && "Problem with the concatenated size");
+ assert(concat_size == state_size_ && "Problem with the concatenated size: " );
// fill the state value from the state concatenation in the order dictated by frame_structure_
int idx = 0;
for (char sb_name: frame_structure_){
if (sb_name == 'O'){
- _state.segment<4>(idx) = states_to_concat_map[sb_name];
- idx += 4;
+ int size_sb = dim_ == 3 ? 4 : 1; // really bad...
+ _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
+ idx += size_sb;
}
else {
- _state.segment<3>(idx) = states_to_concat_map[sb_name];
- idx += 3;
+ int size_sb = dim_ == 3 ? 3 : 2;
+ _state.segment(idx, size_sb) = states_to_concat_map[sb_name];
+ idx += size_sb;
}
}
}
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 4259c0b7c..b0a601c23 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -415,8 +415,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
// Make non-key-frame for last Capture
TimeStamp origin_ts = _origin_frame->getTimeStamp();
auto new_frame_ptr = getProblem()->emplaceFrame(NON_ESTIMATED,
- getProblem()->getState(origin_ts),
+ _origin_frame->getState(),
origin_ts);
+
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
--
GitLab