From 2e92444f37549120d790b9f658ba4aac2be1342c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?M=C3=A9d=C3=A9ric=20Fourmy?= <mfourmy@laas.fr>
Date: Mon, 1 Mar 2021 19:32:34 +0100
Subject: [PATCH] Add default behaviour for processor pose if time tolerance
not set
---
src/processor/processor_pose.cpp | 7 ++++++-
.../gtest_processor_and_factor_pose_3d_with_extrinsics.cpp | 3 ++-
2 files changed, 8 insertions(+), 2 deletions(-)
diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp
index b3cc090df..806021722 100644
--- a/src/processor/processor_pose.cpp
+++ b/src/processor/processor_pose.cpp
@@ -29,7 +29,12 @@ void ProcessorPose::createFactorIfNecessary(){
{
auto kf_it = buffer_pack_kf_.getContainer().begin();
TimeStamp t = kf_it->first;
- auto cap_it = buffer_capture_.selectIterator(t, kf_it->second->time_tolerance);
+ double time_tolerance = std::min(getTimeTolerance(), kf_it->second->time_tolerance);
+ if (getTimeTolerance() == 0.0){
+ WOLF_WARN("Time tolerance set to zero -> value not used");
+ time_tolerance = kf_it->second->time_tolerance;
+ }
+ auto cap_it = buffer_capture_.selectIterator(t, time_tolerance);
// if capture with corresponding timestamp is not found, stop and assume you will get it later
if (cap_it == buffer_capture_.getContainer().end())
diff --git a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
index a81b36918..444fc643c 100644
--- a/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
+++ b/test/gtest_processor_and_factor_pose_3d_with_extrinsics.cpp
@@ -1,7 +1,7 @@
/**
* \file gtest_factor_pose_with_extrinsics.cpp
*
- * Created on: Feb a9, 2020
+ * Created on: Feb 19, 2020
* \author: mfourmy
*/
@@ -116,6 +116,7 @@ class FactorPose3dWithExtrinsicsBase_Test : public testing::Test
Vector7d extr; extr << b_p_bm_, b_q_m_.coeffs();
sensor_pose_ = problem_->installSensor("SensorPose", "pose", extr, intr_sp);
auto params_proc = std::make_shared<ParamsProcessorPose>();
+ params_proc->time_tolerance = 0.5;
auto proc_pose = problem_->installProcessor("ProcessorPose", "pose", sensor_pose_, params_proc);
// somehow by default the sensor extrinsics is fixed
sensor_pose_->unfixExtrinsics();
--
GitLab