WIP Start setPriorOptions with composites

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,

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!");