Skip to content
Snippets Groups Projects
Commit 0cc9b11f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

check user prior keys and small issues

parent 6d740ca8
No related branches found
No related tags found
1 merge request!414Resolve "IsMotion used in Problem::getState()"
Pipeline #6430 passed
...@@ -1023,21 +1023,22 @@ void Problem::setPriorOptions(const std::string& _mode, ...@@ -1023,21 +1023,22 @@ void Problem::setPriorOptions(const std::string& _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");
assert(_state.includesStructure(frame_structure_) && "any missing key in prior state");
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;
assert(prior_options_->state.includesStructure(frame_structure_));
if (prior_options_->mode == "factor") if (prior_options_->mode == "factor")
{ {
assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");
bool isPositive = true; bool isPositive = true;
for(const auto& it: _sigma) for(const auto& it: _sigma)
isPositive = isPositive and (it.second.array() > Constants::EPS).all(); isPositive = isPositive and (it.second.array() > Constants::EPS).all();
assert(isPositive && "sigma is not positive"); assert(isPositive && "prior sigma is not positive");
MatrixComposite Q; MatrixComposite Q;
for (const auto& pair_key_sig : _sigma) for (const auto& pair_key_sig : _sigma)
...@@ -1062,7 +1063,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts) ...@@ -1062,7 +1063,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
if (prior_options_->mode != "nothing" and prior_options_->mode != "") if (prior_options_->mode != "nothing" and prior_options_->mode != "")
{ {
prior_keyframe = emplaceFrame(_ts, prior_options_->state); prior_keyframe = emplaceFrame(_ts, frame_structure_, prior_options_->state);
// Update origin for odometry processors // Update origin for odometry processors
for (auto proc_pair : processor_is_motion_map_) for (auto proc_pair : processor_is_motion_map_)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment