From 77709a08fa40ec22f02200f74e0b10d7a340f11e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Sat, 19 Dec 2020 17:20:15 +0100
Subject: [PATCH] Remove isKey()
---
src/processor/processor_closeloop_icp.cpp | 13 ++++++-------
test/gtest_processor_odom_icp.cpp | 10 ++--------
2 files changed, 8 insertions(+), 15 deletions(-)
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index 0402a4ac6..4fd662693 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -145,13 +145,12 @@ FrameBasePtrList ProcessorCloseloopIcp::selectCandidates(FrameBasePtr _keyframe_
//Consider only key_frames from 1 to n - match_past_key_frame_
// std::copy_if(frames.begin(), frames.end(), std::back_inserter(candidates), [&](FrameBasePtr _frame) { if(_frame->isKey()) {key_frames_counter++; return (key_frames_counter % match_past_key_frame_ == 0);
// }else{ return false;};});
- for(auto it=trajectory->rbegin(); it != trajectory->rend(); it++){
- if((*it)->isKey()){
- // WOLF_DEBUG("TIMESTAMP KEY FRAME ", (*it)->id(), " ", (*it)->getTimeStamp());
- key_frames_counter++;
- if (key_frames_counter > recent_key_frames_ignored_
- and (_keyframe_ptr->getP()->getState() - (*it)->getP()->getState()).norm() < laser_scan_params_.range_max_) candidates.push_back(*it);
- }
+ for(auto it=trajectory->rbegin(); it != trajectory->rend(); it++)
+ {
+ // WOLF_DEBUG("TIMESTAMP KEY FRAME ", (*it)->id(), " ", (*it)->getTimeStamp());
+ key_frames_counter++;
+ if (key_frames_counter > recent_key_frames_ignored_
+ and (_keyframe_ptr->getP()->getState() - (*it)->getP()->getState()).norm() < laser_scan_params_.range_max_) candidates.push_back(*it);
}
WOLF_DEBUG("%%%%%%%%%%%%%%%%%% CANDIDATES SIZE ", candidates.size());
return candidates;
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 64c124fd6..16269a629 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -160,20 +160,14 @@ TEST_F(ProcessorOdomIcp_Test, solve)
}
for (auto F : *problem->getTrajectory())
- {
- if (F->isKey())
- F->perturb(0.5);
- }
+ F->perturb(0.5);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_TRACE(report);
for (auto F : *problem->getTrajectory())
{
- if (F->isKey())
- {
- ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
- }
+ ASSERT_MATRIX_APPROX(F->getState().vector("PO") , x0.vector("PO") , 1e-6);
}
}
--
GitLab