diff --git a/src/problem.cpp b/src/problem.cpp index 63775a5ad6fedd519e993604ea745679c37a3f2d..609f85a25b484bef7b7104fcb62dbafe6dca4a2b 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -330,11 +330,18 @@ bool Problem::permitKeyFrame(ProcessorBasePtr _processor_ptr) void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _processor_ptr, const Scalar& _time_tolerance) { - WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + if (_processor_ptr) + { + WOLF_DEBUG((_processor_ptr->isMotion() ? "PM " : "PT "), _processor_ptr->getName(), ": KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + } + else + { + WOLF_DEBUG("External callback: KF", _keyframe_ptr->id(), " Callback emitted with ts = ", _keyframe_ptr->getTimeStamp()); + } for (auto sensor : hardware_ptr_->getSensorList()) - for (auto processor : sensor->getProcessorList()) - if (processor && (processor != _processor_ptr) ) + for (auto processor : sensor->getProcessorList()) + if (processor && (processor != _processor_ptr) ) processor->keyFrameCallback(_keyframe_ptr, _time_tolerance); } diff --git a/src/test/gtest_IMU.cpp b/src/test/gtest_IMU.cpp index ea51761bbf8d1f59d06cdff71923fc686c302fbd..5c5f48204e39cb1e42f71517fb89cddcf948f561 100644 --- a/src/test/gtest_IMU.cpp +++ b/src/test/gtest_IMU.cpp @@ -503,11 +503,9 @@ class Process_Constraint_IMU : public testing::Test FrameBasePtr KF = problem->emplaceFrame(KEY_FRAME, x1_exact, t); // ===================================== IMU CALLBACK - processor_imu->keyFrameCallback(KF, dt/2); - - - + problem->keyFrameCallback(KF, nullptr, dt/2); + // Process IMU for the callback to take effect data = Vector6s::Zero(); capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov()); processor_imu->process(capture_imu); @@ -685,8 +683,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU Process_Constraint_IMU::buildProblem(); // ===================================== ODO - processor_odo->keyFrameCallback(KF_1, 0.1); - + // Process ODO for the callback to take effect data = Vector6s::Zero(); capture_odo = make_shared<CaptureOdom3D>(t+dt, sensor_odo, data, sensor_odo->getNoiseCov()); processor_odo->process(capture_odo); @@ -1540,7 +1537,7 @@ int main(int argc, char **argv) testing::InitGoogleTest(&argc, argv); // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*"; // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*"; - // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b"; + ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b"; return RUN_ALL_TESTS(); }