From 498a379b131fc1ee3197786b287bf61a94beb828 Mon Sep 17 00:00:00 2001
From: Rainer Kartmann <rainer.kartmann@kit.edu>
Date: Tue, 14 Dec 2021 14:12:30 +0100
Subject: [PATCH] Copy gaussians to robot/global if the original frame matches
---
.../libraries/ArmarXObjects/ObjectPose.cpp | 24 ++++++++++++++++++-
1 file changed, 23 insertions(+), 1 deletion(-)
diff --git a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
index 6b5619647..96cae00af 100644
--- a/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
+++ b/source/RobotAPI/libraries/ArmarXObjects/ObjectPose.cpp
@@ -109,10 +109,32 @@ namespace armarx::objpose
framed.changeFrame(robot, robot->getRootNode()->getName());
objectPoseRobot = framed.toEigen();
objectPoseRobotGaussian = std::nullopt; // ToDo: Transform covariance matrices
+ if (objectPoseOriginalGaussian)
+ {
+ if (objectPoseOriginalFrame == robot->getRootNode()->getName())
+ {
+ objectPoseGlobalGaussian = objectPoseOriginalGaussian;
+ }
+ else
+ {
+ // ToDo: Transform covariance matrices
+ }
+ }
framed.changeToGlobal(robot);
objectPoseGlobal = framed.toEigen();
- objectPoseGlobalGaussian = std::nullopt; // ToDo: Transform covariance matrices
+ objectPoseGlobalGaussian = std::nullopt;
+ if (objectPoseOriginalGaussian)
+ {
+ if (objectPoseOriginalFrame == armarx::GlobalFrame)
+ {
+ objectPoseGlobalGaussian = objectPoseOriginalGaussian;
+ }
+ else
+ {
+ // ToDo: Transform covariance matrices
+ }
+ }
robotConfig = robot->getConfig()->getRobotNodeJointValueMap();
robotPose = robot->getGlobalPose();
--
GitLab