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

[Thomas Ruehr]

I usually do something along that line: First create the three axis 
using cross products and then use them to fill a GL-like rotation matrix 
that is then converted to a Quaternion:

     btMatrix3x3 rot(x_axis.x(), y_axis.x(), z_axis.x(),
                      x_axis.y(), y_axis.y(), z_axis.y(),
                     x_axis.z(), y_axis.z(), z_axis.z());
     btQuaternion rot_quat;
     rot.getRotation(rot_quat);

-tom

On 03/31/2012 01:23 PM, Gonçalo Cabrita wrote:
> 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
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users

-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20120331/5f70fb58/attachment-0004.html>