Skip to content
Snippets Groups Projects
Commit d99c9d76 authored by Joaquim Casals Buñuel's avatar Joaquim Casals Buñuel
Browse files

[WIP] Change signatures of setPrior* methods 

Adapt the signature of all the setPrior* methods to use
Composites instead of plain Eigen structures
parent 5993bac4
No related branches found
No related tags found
1 merge request!366Resolve "Complete state vector new data structure?"
Pipeline #5547 failed
Stage: build
    Hide whitespace changes
    Inline Side-by-side
    include/core/problem/problem.h
    + 9
    − 9
    View file @ d99c9d76
    ...@@ -192,23 +192,23 @@ class Problem : public std::enable_shared_from_this<Problem> ...@@ -192,23 +192,23 @@ class Problem : public std::enable_shared_from_this<Problem>
    // Prior // Prior
    bool isPriorSet() const; bool isPriorSet() const;
    void setPriorOptions(const std::string& _mode, // void setPriorOptions(const std::string& _mode,
    const double _time_tolerance = 0, // const double _time_tolerance = 0,
    const Eigen::VectorXd& _state = Eigen::VectorXd(0), // const Eigen::VectorXd& _state = Eigen::VectorXd(0),
    const Eigen::MatrixXd& _cov = Eigen::MatrixXd(0,0)); // const Eigen::MatrixXd& _cov = Eigen::MatrixXd(0,0));
    void setPriorOptions(const std::string& _mode, void setPriorOptions(const std::string& _mode,
    const double _time_tolerance = 0, const double _time_tolerance = 0,
    const VectorComposite& _state = VectorComposite(), const VectorComposite& _state = VectorComposite(),
    const MatrixComposite& _cov = MatrixComposite()); const VectorComposite& _cov = VectorComposite());
    FrameBasePtr applyPriorOptions(const TimeStamp& _ts); FrameBasePtr applyPriorOptions(const TimeStamp& _ts);
    FrameBasePtr setPriorFactor(const Eigen::VectorXd &_state, FrameBasePtr setPriorFactor(const VectorComposite &_state,
    const Eigen::MatrixXd &_cov, const VectorComposite &_cov,
    const TimeStamp &_ts, const TimeStamp &_ts,
    const double &_time_tol); const double &_time_tol);
    FrameBasePtr setPriorFix(const Eigen::VectorXd &_state, FrameBasePtr setPriorFix(const VectorComposite &_state,
    const TimeStamp &_ts, const TimeStamp &_ts,
    const double &_time_tol); const double &_time_tol);
    FrameBasePtr setPriorInitialGuess(const Eigen::VectorXd &_state, FrameBasePtr setPriorInitialGuess(const VectorComposite &_state,
    const TimeStamp &_ts, const TimeStamp &_ts,
    const double &_time_tol); const double &_time_tol);
    ......
    src/problem/problem.cpp
    + 55
    − 42
    View file @ d99c9d76
    ...@@ -185,15 +185,15 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server) ...@@ -185,15 +185,15 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
    { {
    problem->setPriorOptions(prior_mode, problem->setPriorOptions(prior_mode,
    _server.getParam<double>("problem/prior/time_tolerance"), _server.getParam<double>("problem/prior/time_tolerance"),
    _server.getParam<Eigen::VectorXd>("problem/prior/state"), _server.getParam<VectorComposite>("problem/prior/state"),
    _server.getParam<Eigen::MatrixXd>("problem/prior/cov")); _server.getParam<VectorComposite>("problem/prior/cov"));
    } }
    else else
    { {
    WOLF_TRACE("Prior mode: ", prior_mode); WOLF_TRACE("Prior mode: ", prior_mode);
    problem->setPriorOptions(prior_mode, problem->setPriorOptions(prior_mode,
    _server.getParam<double>("problem/prior/time_tolerance"), _server.getParam<double>("problem/prior/time_tolerance"),
    _server.getParam<Eigen::VectorXd>("problem/prior/state")); _server.getParam<VectorComposite>("problem/prior/state"));
    } }
    // Done // Done
    ...@@ -931,41 +931,41 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons ...@@ -931,41 +931,41 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons
    return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts); return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
    } }
    void Problem::setPriorOptions(const std::string& _mode, // void Problem::setPriorOptions(const std::string& _mode,
    const double _time_tolerance, // const double _time_tolerance,
    const Eigen::VectorXd& _state, // const Eigen::VectorXd& _state,
    const Eigen::MatrixXd& _cov) // const Eigen::MatrixXd& _cov)
    { // {
    assert(prior_options_ != nullptr && "prior options have already been applied"); // assert(prior_options_ != nullptr && "prior options have already been applied");
    assert(prior_options_->mode == "" && "prior options have already been set"); // 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'"); // 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) // // Store options (optionals depending on the mode)
    WOLF_TRACE("prior mode: ", _mode); // WOLF_TRACE("prior mode: ", _mode);
    prior_options_->mode = _mode; // prior_options_->mode = _mode;
    if (prior_options_->mode != "nothing") // if (prior_options_->mode != "nothing")
    { // {
    assert(_time_tolerance > 0 && "time tolerance should be bigger than 0"); // assert(_time_tolerance > 0 && "time tolerance should be bigger than 0");
    WOLF_TRACE("prior state: ", _state.transpose()); // WOLF_TRACE("prior state: ", _state.transpose());
    WOLF_TRACE("prior time tolerance: ", _time_tolerance); // WOLF_TRACE("prior time tolerance: ", _time_tolerance);
    prior_options_->state = _state; // prior_options_->state = _state;
    prior_options_->time_tolerance = _time_tolerance; // prior_options_->time_tolerance = _time_tolerance;
    if (prior_options_->mode == "factor") // if (prior_options_->mode == "factor")
    { // {
    assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)"); // assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)");
    WOLF_TRACE("prior covariance:\n" , _cov); // WOLF_TRACE("prior covariance:\n" , _cov);
    prior_options_->cov = _cov; // prior_options_->cov = _cov;
    } // }
    } // }
    } // }
    void Problem::setPriorOptions(const std::string& _mode, void Problem::setPriorOptions(const std::string& _mode,
    const double _time_tolerance , const double _time_tolerance ,
    const VectorComposite& _state , const VectorComposite& _state ,
    const MatrixComposite& _cov ) const VectorComposite& _sigma )
    { {
    assert(prior_options_ != nullptr && "prior options have already been applied"); assert(prior_options_ != nullptr && "prior options have already been applied");
    assert(prior_options_->mode == "" && "prior options have already been set"); assert(prior_options_->mode == "" && "prior options have already been set");
    ...@@ -974,6 +974,7 @@ void Problem::setPriorOptions(const std::string& _mode, ...@@ -974,6 +974,7 @@ void Problem::setPriorOptions(const std::string& _mode,
    // Store options (optionals depending on the mode) // Store options (optionals depending on the mode)
    WOLF_TRACE("prior mode: ", _mode); WOLF_TRACE("prior mode: ", _mode);
    prior_options_->mode = _mode; prior_options_->mode = _mode;
    prior_options_->structure = getFrameStructure();
    if (prior_options_->mode != "nothing") if (prior_options_->mode != "nothing")
    { {
    ...@@ -981,14 +982,26 @@ void Problem::setPriorOptions(const std::string& _mode, ...@@ -981,14 +982,26 @@ void Problem::setPriorOptions(const std::string& _mode,
    WOLF_TRACE("prior state: ", _state); WOLF_TRACE("prior state: ", _state);
    WOLF_TRACE("prior time tolerance: ", _time_tolerance); WOLF_TRACE("prior time tolerance: ", _time_tolerance);
    prior_options_->state = _state; prior_options_->state__ = _state;
    prior_options_->time_tolerance = _time_tolerance; prior_options_->time_tolerance = _time_tolerance;
    if (prior_options_->mode == "factor") if (prior_options_->mode == "factor")
    { {
    assert(isCovariance(_cov) && "cov is not a covariance matrix (symmetric and Pos Def)"); bool isPositive = true;
    WOLF_TRACE("prior covariance:\n" , _cov); for(const auto& it: _sigma)
    prior_options_->cov = _cov; 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) ...@@ -1065,8 +1078,8 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
    return prior_keyframe; return prior_keyframe;
    } }
    FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state, FrameBasePtr Problem::setPriorFactor(const VectorComposite &_state,
    const Eigen::MatrixXd &_cov, const VectorComposite &_cov,
    const TimeStamp &_ts, const TimeStamp &_ts,
    const double &_time_tol) const double &_time_tol)
    { {
    ...@@ -1075,7 +1088,7 @@ FrameBasePtr Problem::setPriorFactor(const Eigen::VectorXd &_state, ...@@ -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 TimeStamp &_ts,
    const double &_time_tol) const double &_time_tol)
    { {
    ...@@ -1083,7 +1096,7 @@ FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state, ...@@ -1083,7 +1096,7 @@ FrameBasePtr Problem::setPriorFix(const Eigen::VectorXd &_state,
    return applyPriorOptions(_ts); return applyPriorOptions(_ts);
    } }
    FrameBasePtr Problem::setPriorInitialGuess(const Eigen::VectorXd &_state, FrameBasePtr Problem::setPriorInitialGuess(const VectorComposite &_state,
    const TimeStamp &_ts, const TimeStamp &_ts,
    const double &_time_tol) const double &_time_tol)
    { {
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment