check user prior keys and small issues

src/problem/problem.cpp

@@ -1023,21 +1023,22 @@ void Problem::setPriorOptions(const std::string& _mode,
     if (prior_options_->mode != "nothing")
     {
         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 time tolerance: ", _time_tolerance);
         prior_options_->state = _state;
         prior_options_->time_tolerance = _time_tolerance;
 
-        assert(prior_options_->state.includesStructure(frame_structure_));
-
         if (prior_options_->mode == "factor")
         {
+            assert(_sigma.includesStructure(frame_structure_) && "any missing key in prior sigma");
+
             bool isPositive = true;
             for(const auto& it: _sigma)
                 isPositive = isPositive and (it.second.array() > Constants::EPS).all();
 
-            assert(isPositive && "sigma is not positive");
+            assert(isPositive && "prior sigma is not positive");
 
             MatrixComposite Q;
             for (const auto& pair_key_sig : _sigma)
@@ -1062,7 +1063,7 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
 
     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
         for (auto proc_pair : processor_is_motion_map_)