From c9845380180a218ec8cfa0ba9ac189f77d201eb5 Mon Sep 17 00:00:00 2001 From: jcasals <jcasals@iri.upc.edu> Date: Thu, 11 Jun 2020 14:04:39 +0200 Subject: [PATCH] Fix trajectory iterators & emplaceKeyFrame --- src/processor/processor_gnss_fix.cpp | 2 +- src/processor/processor_gnss_single_diff.cpp | 2 +- src/processor/processor_gnss_tdcp.cpp | 6 +++--- test/gtest_factor_gnss_fix_2d.cpp | 2 +- test/gtest_factor_gnss_pseudo_range.cpp | 2 +- test/gtest_factor_gnss_tdcp.cpp | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/processor/processor_gnss_fix.cpp b/src/processor/processor_gnss_fix.cpp index 036a91d51..69ab6723a 100644 --- a/src/processor/processor_gnss_fix.cpp +++ b/src/processor/processor_gnss_fix.cpp @@ -59,7 +59,7 @@ void ProcessorGnssFix::processCapture(CaptureBasePtr _capture) else if (permittedKeyFrame() && voteForKeyFrame()) { WOLF_DEBUG("PR ", getName()," emplacing KF TS = ", incoming_capture_->getTimeStamp()); - new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp()); + new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp()); KF_created = true; } diff --git a/src/processor/processor_gnss_single_diff.cpp b/src/processor/processor_gnss_single_diff.cpp index fb44cb1d7..d2b297a73 100644 --- a/src/processor/processor_gnss_single_diff.cpp +++ b/src/processor/processor_gnss_single_diff.cpp @@ -48,7 +48,7 @@ void ProcessorGnssSingleDiff::processCapture(CaptureBasePtr _capture) // MAKE KF else if (voteForKeyFrame() && permittedKeyFrame()) { - new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp()); + new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp()); getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance); WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp()); } diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp index 7e5b9d904..1734ecf19 100644 --- a/src/processor/processor_gnss_tdcp.cpp +++ b/src/processor/processor_gnss_tdcp.cpp @@ -53,7 +53,7 @@ void ProcessorGnssTdcp::processCapture(CaptureBasePtr _capture) // MAKE KF else if (voteForKeyFrame() && permittedKeyFrame()) { - new_frame = getProblem()->emplaceFrame(KEY, incoming_capture_->getTimeStamp()); + new_frame = getProblem()->emplaceKeyFrame( incoming_capture_->getTimeStamp()); getProblem()->keyFrameCallback(new_frame, shared_from_this(), params_->time_tolerance); WOLF_DEBUG( "PR ",getName()," - capture ", incoming_capture_->id(), " appended to new KF " , new_frame->id() , " TS: ", new_frame->getTimeStamp()); } @@ -123,8 +123,8 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe, const double& _t } // Iterate over all KF of the trajectory with GNSS captures - for (auto KF_it = getProblem()->getTrajectory()->getFrameList().rbegin(); - KF_it != getProblem()->getTrajectory()->getFrameList().rend(); + for (auto KF_it = getProblem()->getTrajectory()->rbegin(); + KF_it != getProblem()->getTrajectory()->rend(); KF_it++) { auto KF_ref = *KF_it; diff --git a/test/gtest_factor_gnss_fix_2d.cpp b/test/gtest_factor_gnss_fix_2d.cpp index b220bbedf..013354b41 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, TimeStamp(0), frame_pose); + frame_ptr = problem_ptr->emplaceKeyFrame( TimeStamp(0), frame_pose); problem_ptr->keyFrameCallback(frame_ptr, nullptr, 1.0); // Create & process GNSS Fix capture diff --git a/test/gtest_factor_gnss_pseudo_range.cpp b/test/gtest_factor_gnss_pseudo_range.cpp index d0568f7de..b98aba78a 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, TimeStamp(0), frm_state); + frm = prb->emplaceKeyFrame( 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 17166ed4b..f0d45e936 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, TimeStamp(0), frm_r_state); + frm_r = prb->emplaceKeyFrame( 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, TimeStamp(1), frm_k_state); + frm_k = prb->emplaceKeyFrame( TimeStamp(1), frm_k_state); // capture r cap_r = CaptureBase::emplace<CaptureGnss>(frm_r, TimeStamp(0), gnss_sensor, nullptr); -- GitLab