From 777a019e893c7404aeebf83834fdf0adaf381289 Mon Sep 17 00:00:00 2001 From: Jose Luis Blanco-Claraco Date: Fri, 13 Dec 2024 15:46:22 +0100 Subject: [PATCH] Allow zero variances for some pose components --- mola_navstate_fuse/src/NavStateFuse.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mola_navstate_fuse/src/NavStateFuse.cpp b/mola_navstate_fuse/src/NavStateFuse.cpp index 88d19e3..32ea680 100644 --- a/mola_navstate_fuse/src/NavStateFuse.cpp +++ b/mola_navstate_fuse/src/NavStateFuse.cpp @@ -83,8 +83,10 @@ void NavStateFuse::fuse_pose( { mrpt::poses::CPose3D incrPose; - // numerical sanity: - for (int i = 0; i < 6; i++) ASSERT_GT_(pose.cov(i, i), .0); + // numerical sanity: variances>=0 (==0 allowed for some components only) + for (int i = 0; i < 6; i++) ASSERT_GE_(pose.cov(i, i), .0); + // and the sum of all strictly >0 + ASSERT_GT_(pose.cov.trace(), .0); double dt = 0; if (state_.last_pose_obs_tim)