From 0cc9b11f1ff22403fa462c639a1f1d9c35f373d0 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 5 Mar 2021 16:01:50 +0100
Subject: [PATCH] check user prior keys and small issues
---
src/problem/problem.cpp | 9 +++++----
1 file changed, 5 insertions(+), 4 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 121ffc873..0bb18fd52 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1023,21 +1023,22 @@ void Problem::setPriorOptions(const std::string& _mode,
if (prior_options_->mode != "nothing")
{
assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+ assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
WOLF_TRACE("prior state: ", _state);
WOLF_TRACE("prior time tolerance: ", _time_tolerance);
prior_options_->state = _state;
prior_options_->time_tolerance = _time_tolerance;
- assert(prior_options_->state.includesStructure(frame_structure_));
-
if (prior_options_->mode == "factor")
{
+ assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");
+
bool isPositive = true;
for(const auto& it: _sigma)
isPositive = isPositive and (it.second.array() > Constants::EPS).all();
- assert(isPositive && "sigma is not positive");
+ assert(isPositive && "prior sigma is not positive");
MatrixComposite Q;
for (const auto& pair_key_sig : _sigma)
@@ -1062,7 +1063,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{
- prior_keyframe = emplaceFrame(_ts, prior_options_->state);
+ prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
// Update origin for odometry processors
for (auto proc_pair : processor_is_motion_map_)
--
GitLab