From 162195e420eb69ccbccd1075947ef18c0668caa2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Thu, 4 Jun 2020 11:12:11 +0200
Subject: [PATCH] WIP Start setPriorOptions with composites
---
include/core/problem/problem.h | 4 ++++
src/problem/problem.cpp | 31 +++++++++++++++++++++++++++++++
2 files changed, 35 insertions(+)
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 2ce8f522b..0ea927f3c 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -193,6 +193,10 @@ class Problem : public std::enable_shared_from_this<Problem>
const double _time_tolerance = 0,
const Eigen::VectorXd& _state = Eigen::VectorXd(0),
const Eigen::MatrixXd& _cov = Eigen::MatrixXd(0,0));
+ void setPriorOptions(const std::string& _mode,
+ const double _time_tolerance = 0,
+ const VectorComposite& _state = VectorComposite(),
+ const MatrixComposite& _cov = MatrixComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
FrameBasePtr setPriorFactor(const Eigen::VectorXd &_state,
const Eigen::MatrixXd &_cov,
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 17e2418fb..1c5dc6e32 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -962,6 +962,37 @@ void Problem::setPriorOptions(const std::string& _mode,
}
}
+void Problem::setPriorOptions(const std::string& _mode,
+ const double _time_tolerance ,
+ const VectorComposite& _state ,
+ const MatrixComposite& _cov )
+{
+// assert(prior_options_ != nullptr && "prior options have already been applied");
+// assert(prior_options_->mode == "" && "prior options have already been set");
+// assert((_mode == "nothing" || _mode == "initial_guess" || _mode == "fix" || _mode == "factor") && "wrong _mode value, it should be: 'nothing', 'initial_guess', 'fix' or 'factor'");
+//
+// // Store options (optionals depending on the mode)
+// WOLF_TRACE("prior mode: ", _mode);
+// prior_options_->mode = _mode;
+//
+// if (prior_options_->mode != "nothing")
+// {
+// assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
+//
+// WOLF_TRACE("prior state: ", _state.transpose());
+// WOLF_TRACE("prior time tolerance: ", _time_tolerance);
+// prior_options_->state = _state;
+// prior_options_->time_tolerance = _time_tolerance;
+//
+// if (prior_options_->mode == "factor")
+// {
+// assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)");
+// WOLF_TRACE("prior covariance:\n" , _cov);
+// prior_options_->cov = _cov;
+// }
+// }
+}
+
FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
{
assert(!isPriorSet() && "applyPriorOptions can be called once!");
--
GitLab