diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index cfedb4da63bf0dcf91acdc12f0a5c13d05219bde..c8a3990106092089678577bf6e0ce4c6e9f41545 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -135,8 +135,8 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture) if ( sensor_gnss_->isEnuDefined() && !sensor_gnss_->isEnuMapInitialized() ) { WOLF_DEBUG("(re-)initializing enu map"); - sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState(), first_feature_->getMeasurement(), - incoming_capture_->getFrame()->getState(), incoming_feature->getMeasurement()); + sensor_gnss_->initializeEnuMap(first_capture_->getFrame()->getState().vector("PO"), first_feature_->getMeasurement(), + incoming_capture_->getFrame()->getState().vector("PO"), incoming_feature->getMeasurement()); // Set as not-initialized if factors not separated enough ( < enu_map_init_dist_min) if ((first_feature_->getMeasurement() - incoming_feature->getMeasurement()).norm() < params_gnss_->enu_map_init_dist_min) sensor_gnss_->setEnuMapInitialized(false); diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp index 35be5420d280579442d2aef6d97a18233e7474d9..2f2b937e88316158580b912c0267be8d91a4eee1 100644 --- a/src/processor/processor_tracker_gnss.cpp +++ b/src/processor/processor_tracker_gnss.cpp @@ -9,7 +9,7 @@ namespace wolf { ProcessorTrackerGnss::ProcessorTrackerGnss(ParamsProcessorTrackerGnssPtr _params_tracker_gnss) : - ProcessorTrackerFeature("ProcessorTrackerGnss", 3, _params_tracker_gnss), + ProcessorTrackerFeature("ProcessorTrackerGnss", "PO", 3, _params_tracker_gnss), params_tracker_gnss_(_params_tracker_gnss), outliers_pseudorange_(0), outliers_tdcp_(0), diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index b3a1ec3f34973d3037fa8f50f9d087cd37ea73f0..b220bbedf106e0bf6c1912ac0abffe28725ae2fc 100644 --- a/test/gtest_factor_gnss_fix_2d.cpp +++ b/test/gtest_factor_gnss_fix_2d.cpp @@ -112,7 +112,7 @@ TEST(FactorGnssFix2dTest, configure_tree) // Emplace a frame (FIXED) Vector3d frame_pose = (Vector3d() << t_map_base(0), t_map_base(1), o_map_base(0)).finished(); - frame_ptr = problem_ptr->emplaceFrame(KEY, frame_pose, TimeStamp(0)); + frame_ptr = problem_ptr->emplaceFrame(KEY, TimeStamp(0), frame_pose); problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0); // Create & process GNSS Fix capture @@ -140,10 +140,10 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) frame_ptr->getP()->unfix(); // --------------------------- distort: map base position - Vector3d frm_dist = frame_ptr->getState(); + Vector3d frm_dist = frame_ptr->getState().vector("PO"); frm_dist(0) += 0.18; frm_dist(1) += -0.58; - frame_ptr->setState(frm_dist); + frame_ptr->setState(frm_dist, "PO", {2,1}); // --------------------------- update solver ceres_mgr_ptr->update(); @@ -167,7 +167,7 @@ TEST(FactorGnssFix2dTest, gnss_1_map_base_position) ASSERT_EQ(ceres_mgr_ptr->getSummary().num_residuals_reduced, 3); // --------------------------- check solver solution - ASSERT_MATRIX_APPROX(frame_ptr->getState().head(2), t_map_base.head(2), 1e-6); + ASSERT_MATRIX_APPROX(frame_ptr->getState().at("P"), t_map_base.head(2), 1e-6); } /* diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index 662bc956a9b526c47bf8172ffcfab1c7c3425054..d0568f7deb1d9eea3ce1c789f5cdd6ac3127591a 100644 --- a/test/gtest_factor_gnss_pseudo_range.cpp +++ b/test/gtest_factor_gnss_pseudo_range.cpp @@ -104,7 +104,7 @@ void setUpProblem() Vector7d frm_state; frm_state.head<3>() = t_map_base; frm_state.tail<4>() = q_map_base.coeffs(); - frm = prb->emplaceFrame(KEY, frm_state, TimeStamp(0)); + frm = prb->emplaceFrame(KEY, TimeStamp(0), frm_state); // capture cap = CaptureBase::emplace<CaptureGnss>(frm, TimeStamp(0), gnss_sensor, nullptr); diff --git a/test/gtest_factor_gnss_tdcp.cpp b/test/gtest_factor_gnss_tdcp.cpp index 0e7b697e7175a529c898f2261a2a1d28ec39aa40..17166ed4ba63424202fd6c4949a5d5c8295b0f86 100644 --- a/test/gtest_factor_gnss_tdcp.cpp +++ b/test/gtest_factor_gnss_tdcp.cpp @@ -118,13 +118,13 @@ void setUpProblem() Vector7d frm_r_state; frm_r_state.head<3>() = t_map_base_r; frm_r_state.tail<4>() = q_map_base_r.coeffs(); - frm_r = prb->emplaceFrame(KEY, frm_r_state, TimeStamp(0)); + frm_r = prb->emplaceFrame(KEY, TimeStamp(0), frm_r_state); // Frame k Vector7d frm_k_state; frm_k_state.head<3>() = t_map_base_k; frm_k_state.tail<4>() = q_map_base_k.coeffs(); - frm_k = prb->emplaceFrame(KEY, frm_k_state, TimeStamp(1)); + frm_k = prb->emplaceFrame(KEY, TimeStamp(1), frm_k_state); // capture r cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr);