From 147a8a0dacc032e8a7c22f1a519564766af35613 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 30 Jul 2018 16:10:09 -0400 Subject: [PATCH] refactored tf generation --- src/ros/FindObjectROS.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/ros/FindObjectROS.cpp b/src/ros/FindObjectROS.cpp index 092ade9c..841dc5e3 100644 --- a/src/ros/FindObjectROS.cpp +++ b/src/ros/FindObjectROS.cpp @@ -110,12 +110,13 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info) transform.stamp_ = stamp_; transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); - //set rotation (y inverted) + //set rotation tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]); xAxis.normalize(); tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]); yAxis.normalize(); - tf::Vector3 zAxis = xAxis*yAxis; + tf::Vector3 zAxis = xAxis.cross(yAxis); + zAxis.normalize(); tf::Matrix3x3 rotationMatrix( xAxis.x(), yAxis.x() ,zAxis.x(), xAxis.y(), yAxis.y(), zAxis.y(),