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

[Gonçalo Cabrita]

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
        if(t>max_t)
        {
            max_t = t;
            max_p = p;
        }
        if(t<min_t)
        {
            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);
    base_vector.normalized();
    tf::Vector3 top_vector(top_pt.x, top_pt.y, top_pt.z);
    top_vector.normalized();
    tf::Vector3 cross_vector = base_vector.cross(top_vector);
    tf::Quaternion q(cross_vector, base_vector.dot(top_vector) + 1.0);
    q.normalize();    
    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>