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@code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
(text/plain)