diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 328d3d1c66af0b316bb3187564cb279fd695f664..0d8539fd05d64803327ab3ceafba4feb0763d1c3 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -192,23 +192,23 @@ class Problem : public std::enable_shared_from_this<Problem>
// Prior
bool isPriorSet() const;
- void setPriorOptions(const std::string& _mode,
- 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 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());
+ const VectorComposite& _cov = VectorComposite());
FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
- FrameBasePtr setPriorFactor(const Eigen::VectorXd &_state,
- const Eigen::MatrixXd &_cov,
+ FrameBasePtr setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_cov,
const TimeStamp &_ts,
const double &_time_tol);
- FrameBasePtr setPriorFix(const Eigen::VectorXd &_state,
+ FrameBasePtr setPriorFix(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol);
- FrameBasePtr setPriorInitialGuess(const Eigen::VectorXd &_state,
+ FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol);
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index de40f101d1a6bf8a2285294a7aec5828139ce061..d0b977b81653889b4112ec7bf90481753ef7ddd2 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -185,15 +185,15 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
{
problem->setPriorOptions(prior_mode,
_server.getParam<double>("problem/prior/time_tolerance"),
- _server.getParam<Eigen::VectorXd>("problem/prior/state"),
- _server.getParam<Eigen::MatrixXd>("problem/prior/cov"));
+ _server.getParam<VectorComposite>("problem/prior/state"),
+ _server.getParam<VectorComposite>("problem/prior/cov"));
}
else
{
WOLF_TRACE("Prior mode: ", prior_mode);
problem->setPriorOptions(prior_mode,
_server.getParam<double>("problem/prior/time_tolerance"),
- _server.getParam<Eigen::VectorXd>("problem/prior/state"));
+ _server.getParam<VectorComposite>("problem/prior/state"));
}
// Done
@@ -931,41 +931,41 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons
return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}
-void Problem::setPriorOptions(const std::string& _mode,
- const double _time_tolerance,
- const Eigen::VectorXd& _state,
- const Eigen::MatrixXd& _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;
- }
- }
-}
+// void Problem::setPriorOptions(const std::string& _mode,
+// const double _time_tolerance,
+// const Eigen::VectorXd& _state,
+// const Eigen::MatrixXd& _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;
+// }
+// }
+// }
void Problem::setPriorOptions(const std::string& _mode,
const double _time_tolerance ,
const VectorComposite& _state ,
- const MatrixComposite& _cov )
+ const VectorComposite& _sigma )
{
assert(prior_options_ != nullptr && "prior options have already been applied");
assert(prior_options_->mode == "" && "prior options have already been set");
@@ -974,6 +974,7 @@ void Problem::setPriorOptions(const std::string& _mode,
// Store options (optionals depending on the mode)
WOLF_TRACE("prior mode: ", _mode);
prior_options_->mode = _mode;
+ prior_options_->structure = getFrameStructure();
if (prior_options_->mode != "nothing")
{
@@ -981,14 +982,26 @@ void Problem::setPriorOptions(const std::string& _mode,
WOLF_TRACE("prior state: ", _state);
WOLF_TRACE("prior time tolerance: ", _time_tolerance);
- prior_options_->state = _state;
+ 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;
+ bool isPositive = true;
+ for(const auto& it: _sigma)
+ isPositive = isPositive and (it.second.array() > Constants::EPS).all();
+
+ assert(isPositive && "sigma is not positive");
+
+ MatrixComposite Q;
+ for (const auto& ckey : prior_options_->structure)
+ {
+ const auto& key = string(1,ckey); // ckey is char
+ auto cov_blk = (_sigma.at(key).array() * _sigma.at(key).array()).matrix();
+ Q.emplace(key,key,cov_blk);
+ }
+ WOLF_TRACE("prior covariance:\n" , Q);
+ prior_options_->cov__ = Q;
}
}
}
@@ -1065,8 +1078,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
return prior_keyframe;
}
-FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state,
- const Eigen::MatrixXd &_cov,
+FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
+ const VectorComposite &_cov,
const TimeStamp &_ts,
const double &_time_tol)
{
@@ -1075,7 +1088,7 @@ FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state,
}
-FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
+FrameBasePtr Problem::setPriorFix(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol)
{
@@ -1083,7 +1096,7 @@ FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
return applyPriorOptions(_ts);
}
-FrameBasePtr Problem::setPriorInitialGuess(const Eigen::VectorXd &_state,
+FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
const TimeStamp &_ts,
const double &_time_tol)
{