From 0ca047a7e5d6fd2f4c8f138d5e54b0272eeb7a15 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 5 Jun 2020 11:52:01 +0200
Subject: [PATCH] WIP fixing applyPriorOptions()
---
src/problem/problem.cpp | 69 ++++++++++++++++++++++++++++++-----------
1 file changed, 51 insertions(+), 18 deletions(-)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index d0b977b81..ddda01d42 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -1016,35 +1016,37 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
prior_keyframe = emplaceFrame(KEY,
_ts,
- this->getFrameStructure(),
- this->getDim(),
- prior_options_->state);
+ prior_options_->state__);
if (prior_options_->mode == "fix")
prior_keyframe->fix();
else if (prior_options_->mode == "factor")
{
- // FIXME: change whenever state is changed to a map<string,vector>
// ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
// Emplace a capture
auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
// Emplace a feature and a factor for each state block
- int state_idx = 0;
- int cov_idx = 0;
- for (auto sb : prior_keyframe->getStateBlockVec())
+ for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
{
- assert(sb != nullptr);
- assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
- assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
+ const auto& key = pair_key_sb.first;
+ const auto& sb = pair_key_sb.second;
- // state block segment
- Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
- Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+ const auto& state_blk = prior_options_->state__.at(key);
+ const auto& covar_blk = prior_options_->cov__.at(key,key);
+
+ assert(sb->getSize() == state_blk.size() && "prior_options state wrong size");
+ assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
+
+ WOLF_DEBUG("key: ", key);
+ WOLF_DEBUG("vec: ", state_blk);
+ WOLF_DEBUG("cov: ", covar_blk);
+ WOLF_DEBUG("sb : ", sb->getState());
+ WOLF_DEBUG("----------------------: ");
// feature
- auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+ auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_blk, covar_blk);
// factor
if (sb->hasLocalParametrization())
@@ -1060,11 +1062,42 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
}
- state_idx += sb->getSize();
- cov_idx += sb->getLocalSize();
+
}
- assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
- assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
+
+
+// for (auto sb : prior_keyframe->getStateBlockVec())
+// {
+// assert(sb != nullptr);
+// assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
+// assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
+//
+// // state block segment
+// Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
+// Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+//
+// // feature
+// auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+//
+// // factor
+// if (sb->hasLocalParametrization())
+// {
+// if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
+// auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
+// else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
+// auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+// else
+// throw std::runtime_error("not implemented...!");
+// }
+// else
+// {
+// auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+// }
+// state_idx += sb->getSize();
+// cov_idx += sb->getLocalSize();
+// }
+// assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
+// assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
}
else
assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");
--
GitLab