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