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>
    // 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);
    ......
    src/problem/problem.cpp
    + 55
    − 42
    View file @ d99c9d76
    ......@@ -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)
    {
    ......
    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