WIP fixing applyPriorOptions()

src/problem/problem.cpp

@@ -1016,35 +1016,37 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
     {
         prior_keyframe = emplaceFrame(KEY,
                                       _ts,
-                                      this->getFrameStructure(),
-                                      this->getDim(),
-                                      prior_options_->state);
+                                      prior_options_->state__);
 
         if (prior_options_->mode == "fix")
             prior_keyframe->fix();
         else if (prior_options_->mode == "factor")
         {
-            // FIXME: change whenever state is changed to a map<string,vector>
             // ASSUMPTION: Independent measurements (non-correlated block-diagonal covariance matrix)
 
             // Emplace a capture
             auto prior_cap = CaptureBase::emplace<CaptureVoid>(prior_keyframe, _ts, nullptr);
 
             // Emplace a feature and a factor for each state block
-            int state_idx = 0;
-            int cov_idx = 0;
-            for (auto sb : prior_keyframe->getStateBlockVec())
+            for (const auto& pair_key_sb : prior_keyframe->getStateBlockMap())
             {
-                assert(sb != nullptr);
-                assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
-                assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
+                const auto& key = pair_key_sb.first;
+                const auto& sb  = pair_key_sb.second;
 
-                // state block segment
-                Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
-                Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+                const auto& state_blk = prior_options_->state__.at(key);
+                const auto& covar_blk = prior_options_->cov__.at(key,key);
+
+                assert(sb->getSize()      == state_blk.size() && "prior_options state wrong size");
+                assert(sb->getLocalSize() == covar_blk.rows() && "prior_options cov. wrong size");
+
+                WOLF_DEBUG("key: ", key);
+                WOLF_DEBUG("vec: ", state_blk);
+                WOLF_DEBUG("cov: ", covar_blk);
+                WOLF_DEBUG("sb : ", sb->getState());
+                WOLF_DEBUG("----------------------: ");
 
                 // feature
-                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_blk, covar_blk);
 
                 // factor
                 if (sb->hasLocalParametrization())
@@ -1060,11 +1062,42 @@ FrameBasePtr Problem::applyPriorOptions(const TimeStamp& _ts)
                 {
                     auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
                 }
-                state_idx += sb->getSize();
-                cov_idx += sb->getLocalSize();
+
             }
-            assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
-            assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
+
+
+//            for (auto sb : prior_keyframe->getStateBlockVec())
+//            {
+//                assert(sb != nullptr);
+//                assert(state_idx+sb->getSize() <= prior_options_->state.size() && "prior_options state wrong size (dimension too small)");
+//                assert(cov_idx+sb->getLocalSize() <= prior_options_->cov.rows() && "prior_options cov wrong size (dimension too small)");
+//
+//                // state block segment
+//                Eigen::VectorXd state_segment = prior_options_->state.segment(state_idx, sb->getSize());
+//                Eigen::MatrixXd cov_block = prior_options_->cov.block(cov_idx, cov_idx, sb->getLocalSize(), sb->getLocalSize());
+//
+//                // feature
+//                auto prior_fea = FeatureBase::emplace<FeatureBase>(prior_cap, "prior", state_segment, cov_block);
+//
+//                // factor
+//                if (sb->hasLocalParametrization())
+//                {
+//                    if (std::dynamic_pointer_cast<StateQuaternion>(sb) != nullptr)
+//                        auto prior_fac = FactorBase::emplace<FactorQuaternionAbsolute>(prior_fea, sb, nullptr, false);
+//                    else if (std::dynamic_pointer_cast<StateAngle>(sb) != nullptr)
+//                        auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+//                    else
+//                        throw std::runtime_error("not implemented...!");
+//                }
+//                else
+//                {
+//                    auto prior_fac = FactorBase::emplace<FactorBlockAbsolute>(prior_fea, sb, nullptr, false);
+//                }
+//                state_idx += sb->getSize();
+//                cov_idx += sb->getLocalSize();
+//            }
+//            assert(state_idx == prior_options_->state.size() && "prior_options state wrong size (dimension too big)");
+//            assert(cov_idx == prior_options_->cov.rows() && "prior_options cov wrong size (dimension too big)");
         }
         else
             assert(prior_options_->mode == "initial_guess" && "wrong prior_options->mode");