feat(object_recognition_utils): implement covariance transform in the…

… transformObjects (autowarefoundation#8953) * feat: implement covariance transform in the transformObjects Signed-off-by: Taekjin LEE <[email protected]> * fix: transform covariance is only the frame difference Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
technolojin · Sep 26, 2024 · 17bf3ad · 17bf3ad
1 parent 65a36d4
commit 17bf3ad
Showing 1 changed file with 7 additions and 3 deletions.
diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp
@@ -95,10 +95,14 @@ bool transformObjects(
       tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world);
     }
     for (auto & object : output_msg.objects) {
-      tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects);
+      auto & pose_with_cov = object.kinematics.pose_with_covariance;
+      tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects);
       tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
-      tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose);
-      // TODO(yukkysaito) transform covariance
+      // transform pose, frame difference and object pose
+      tf2::toMsg(tf_target2objects, pose_with_cov.pose);
+      // transform covariance, only the frame difference
+      pose_with_cov.covariance =
+        tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world);
     }
   }
   return true;