From 8634588ac3b8a8a544563ad7dc7e1add3f92df58 Mon Sep 17 00:00:00 2001 From: Rik Bouwmeester Date: Tue, 20 Jan 2026 16:26:44 +0100 Subject: [PATCH] Fix quaternion averaging Use np.linalg.eigh instead of np.linalg.eig for the symmetric matrix Q.T @ Q. eigh is designed for symmetric matrices and guarantees real eigenvalues/eigenvectors, whereas eig can return complex values due to numerical precision. This fixes a dtype mismatch error in scipy's Rotation.from_quat which now strictly validates input types. --- cflib/localization/lighthouse_initial_estimator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cflib/localization/lighthouse_initial_estimator.py b/cflib/localization/lighthouse_initial_estimator.py index 722e66d1e..70778b1a9 100644 --- a/cflib/localization/lighthouse_initial_estimator.py +++ b/cflib/localization/lighthouse_initial_estimator.py @@ -373,9 +373,9 @@ def _avarage_poses(cls, poses: list[Pose]) -> Pose: """ def q_average(Q, W=None): if W is not None: - Q *= W[:, None] - eigvals, eigvecs = np.linalg.eig(Q.T@Q) - return eigvecs[:, eigvals.argmax()] + Q = Q * W[:, None] + _, eigvecs = np.linalg.eigh(Q.T @ Q) + return eigvecs[:, -1] positions = map(lambda x: x.translation, poses) average_pos = np.average(np.array(list(positions)), axis=0)