From 63a04ade174e60830698eb1c36f6c3978cbe99b8 Mon Sep 17 00:00:00 2001 From: Tanmay Date: Thu, 10 Apr 2025 15:35:27 +0200 Subject: [PATCH] fix: pose average z coord becomes zero --- include/cmr_geometry_utils/pose.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/cmr_geometry_utils/pose.hpp b/include/cmr_geometry_utils/pose.hpp index 8600dac..4d4334d 100644 --- a/include/cmr_geometry_utils/pose.hpp +++ b/include/cmr_geometry_utils/pose.hpp @@ -14,12 +14,14 @@ namespace pose { for(const auto& pose : poses){ average_pose.position.x += pose.position.x; average_pose.position.y += pose.position.y; + average_pose.position.z += pose.position.z; quats.push_back(pose.orientation); } average_pose.position.x /= poses.size(); average_pose.position.y /= poses.size(); + average_pose.position.z /= poses.size(); cmr_geometry_utils::quaternion::average(quats, average_pose.orientation);