converting tf to eign vector
I am trying to convert the camera tf to Eigen::Vector3d for some purpose. Here is my code:
tf::TransformListener listener;
tf::StampedTransform transform;
try{
listener.lookupTransform("/map", "/camera_link",
ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
Eigen::Isometry3d camera_vec;
tf::poseTFToEigen(transform, camera_vec);
for(auto point : g_path->poses) {
Eigen::Vector3d direction = Eigen::Vector3d(point.pose.position.x, point.pose.position.y, point.pose.position.z) -
Eigen::Vector3d(transform.getOrigin().x(),transform.getOrigin().y(),transform.getOrigin().z());
auto cross_prod = camera_vec.cross(direction);
float pitch = std::atan2(cross_prod.norm(), p0p1.dot(ref_y_axis));
head_motion_cmd.data = -1.0;
head_motion_cmd_pub.publish(head_motion_cmd);
rate->sleep();
}
Here i only found tf::poseTFToEigen
which converts tf to Eigen::Isometry3d. But to calculate the angle between this vector and anothor called direction i need this vector also in Eigen::Vector3d form which looks like is not supported by this function.
Any idea which function to use?
Environment: ROS notic Ubuntu 20.04
Asked by dinesh on 2022-01-12 02:07:36 UTC
Comments
Hi @dinesh, is it possible to use tf2::convert instead? http://wiki.ros.org/tf2/Tutorials/Migration/DataConversions
Asked by osilva on 2022-01-12 17:14:36 UTC