converting tf to eign vector

asked 2022-01-12 01:07:36 -0500

dinesh gravatar image

updated 2022-01-12 01:08:00 -0500

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

edit retag flag offensive close merge delete

Comments

Hi @dinesh, is it possible to use tf2::convert instead? http://wiki.ros.org/tf2/Tutorials/Mig...

osilva gravatar image osilva  ( 2022-01-12 16:14:36 -0500 )edit