diff --git a/src/processor/processor_pose.cpp b/src/processor/processor_pose.cpp index b3cc090dfe29d9d875af68ed51810a56093b9efb..806021722dd5ad3b25220315a71cc578f02ed0bf 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 a81b36918ce96fa8d47d6387cdae5a24a6a95800..444fc643caa29559441c9c64911048285a344989 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();