apoorva/greenhouse
2
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

refactored tf generation

Browse Source
This commit is contained in:
matlabbe 2018-07-30 16:10:09 -04:00
parent cb349e557f
commit 147a8a0dac
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/ros/FindObjectROS.cpp
View File

@ -110,12 +110,13 @@ void FindObjectROS::publish(const find_object::DetectionInfo & info)
transform.stamp_ = stamp_; transform.stamp_ = stamp_;
transform.setOrigin(tf::Vector3(center3D.val[0], center3D.val[1], center3D.val[2])); 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]); tf::Vector3 xAxis(axisEndX.val[0] - center3D.val[0], axisEndX.val[1] - center3D.val[1], axisEndX.val[2] - center3D.val[2]);
xAxis.normalize(); xAxis.normalize();
tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]); tf::Vector3 yAxis(axisEndY.val[0] - center3D.val[0], axisEndY.val[1] - center3D.val[1], axisEndY.val[2] - center3D.val[2]);
yAxis.normalize(); yAxis.normalize();
tf::Vector3 zAxis = xAxis*yAxis; tf::Vector3 zAxis = xAxis.cross(yAxis);
zAxis.normalize();
tf::Matrix3x3 rotationMatrix( tf::Matrix3x3 rotationMatrix(
xAxis.x(), yAxis.x() ,zAxis.x(), xAxis.x(), yAxis.x() ,zAxis.x(),
xAxis.y(), yAxis.y(), zAxis.y(), xAxis.y(), yAxis.y(), zAxis.y(),