[ros-users] tf::Vector3 to look at tf::Quaternion

Gonçalo Cabrita goncabrita at gmail.com
Sat Mar 31 11:23:13 UTC 2012

Hi everyone!

My program uses a Kinect to extract cylinders using PCL and draws them in rviz, or at least its supposed to! The problem is that the model from PCL provides me with a Vector3 that represents the axis of the cylinder and to draw it as a Marker on rviz I need to convert this into a quaternion. I also have access to a random point along the axis of the cylinder so I made the following function to extract the top and bottom points of the axis of the cylinder:

void getMinMax3DalongAxis(const PointCloud::ConstPtr& cloud, PointT * max_pt, PointT * min_pt, PointT * axis_point, tf::Vector3 * normal)
    PointT max_p = *axis_point;
    double max_t = 0.0;
    PointT min_p = *axis_point;
    double min_t = 0.0;
    BOOST_FOREACH(const PointT& pt, cloud->points)
        // First we convert pt into a point p along the axis on the same plane perpendicular to the axis
        double t = (normal->x()*(pt.x-axis_point->x) + normal->y()*(pt.y-axis_point->y) + normal->z()*(pt.z-axis_point->z))/(pow(normal->x(),2) + pow(normal->y(),2) + pow(normal->z(),2));
        PointT p;
        p.x = axis_point->x + normal->x() * t;
        p.y = axis_point->y + normal->y() * t;
        p.z = axis_point->z + normal->z() * t;
        // Now we check if the point is min or max using the result of the parametric equations
            max_t = t;
            max_p = p;
            min_t = t;
            min_p = p;
    *max_pt = max_p;
    *min_pt = min_p;

I believe this to be working properly as I am showing the returned dots as spheres on rviz. The problem I believe lies on the conversion I'm making from the Vector3 to Quaternion. I am aware that both things cannot hold the same information, I am trying to calculate the look at quaternion given to me by the vector that represents the axis of the cylinder that provides me a direction and not a rotation. This is the piece of software that I believe is resulting in a bad representation of the cylinder in rviz:

    // Calculate the look at quaternion of the cylinder axis
    tf::Vector3 base_vector(base_pt.x, base_pt.y, base_pt.z);
    tf::Vector3 top_vector(top_pt.x, top_pt.y, top_pt.z);
    tf::Vector3 cross_vector = base_vector.cross(top_vector);
    tf::Quaternion q(cross_vector, base_vector.dot(top_vector) + 1.0);
    geometry_msgs::Quaternion cylinder_orientation;
    tf::quaternionTFToMsg(q, cylinder_orientation);

Would this be a good question of answers.ros.og? Perhaps if I make it more general in terms of the conversion between Vector3 and a look at Quaternion?

Thanks in advance,

Gonçalo Cabrita
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.osuosl.org/pipermail/ros-users/attachments/20120331/24661aef/attachment-0003.html>

More information about the ros-users mailing list