CODEWITHSUNDEEP
c++rospoint-cloud-libraryroboticsrviz
DuffRumkins
DuffRumkins

Reputation: 13

ROS RVIZ: How to visualize PCL Model with correct orientation?

I am trying to visualize a cylinder that I fit to a point cloud using the PCL RANSAC functions. When I fit the model I am given a pcl::ModelCoefficients object and this has the following coefficients point_on_axis, axis_direction, cylinder_radius_R, see documentation here.

As I understand it, the axis direction values (values 3, 4 and 5 of the coefficient object values member) are the x,y and z components of the cylinder axis vector, respectively. To display this vector in RVIZ using a marker object, I need to convert this vector to a quarternion. I have been doing this using the following c++ code:

        //Convert axis vector to quarternion format
        double axis_pitch = atan2(coefficients_cylinder.values[5],coefficients_cylinder.values[4]);
        double axis_roll = atan2(coefficients_cylinder.values[3],coefficients_cylinder.values[5]);
        double axis_yaw = atan2(coefficients_cylinder.values[3],coefficients_cylinder.values[4]);
        tf2::Quaternion axis_quarternion;
        axis_quarternion.setRPY( axis_roll, axis_pitch, axis_yaw );
        axis_quarternion.normalize();

However, whenever I view the cylinder markers overlayed on the original point cloud they have the wrong orientation.

What is causing this? Am I missing a step in the conversion or is my approach completely off?

Thanks for the help!

EDIT:

I found the following code to work quite well:

        //Convert axis vector to quarternion format
        double axis_roll = atan2(coefficients_cylinder->values[5],coefficients_cylinder->values[4]);
        double axis_pitch = -1.0 * atan2(coefficients_cylinder->values[5],coefficients_cylinder->values[3]);
        double axis_yaw = atan2(coefficients_cylinder->values[4],coefficients_cylinder->values[3]);
        tf2::Quaternion axis_quarternion;
        axis_quarternion.setRPY( 0.0, -0.5*M_PI + axis_pitch, axis_yaw);
        // axis_quarternion.setRPY( axis_roll, axis_pitch, axis_yaw);
        axis_quarternion.normalize();

I had made some incorrect assumptions on the axis directions (see mgrova's answer) and not taken into account that by default the rviz cylinder marker is aligned with the z-axis.

Upvotes: 0

Views: 429

Answers (1)

mgrova
mgrova

Reputation: 11

I think that one problem is that PCL works with an optical axis frame (right hand definition: x=down, y=left, z=forward).

That's why the best way that I find to be able to represent this orientation is obtaining the Eigen::QuaternionX from the Eigen::AngleAxis giving an angle and the axis where it was measured.

Try with this function to convert from RANSAC cylinder model to geometry_msgs/Quaternion. This orientation will be used to display the cylinder marker in Rviz.

#include <tf2_eigen/tf2_eigen.h>

inline Eigen::Quaterniond obtainCylinderOrientationFromModel(
        const pcl::ModelCoefficients::ConstPtr& coefficients)
{
    Eigen::Vector3d axis_vector(coefficients->values[3],
                                coefficients->values[4],
                                coefficients->values[5]);
    Eigen::Vector3d up_vector(0.0, 0.0, -1.0);
    Eigen::Vector3d right_vector = axis_vector.cross(up_vector);
    right_vector.normalized();

    Eigen::Quaterniond q(Eigen::AngleAxisd(-1.0 * std::acos(axis_vector.dot(up_vector)), 
                                           right_vector));
    q.normalize();

    return q;
}

...

// Obtain geometry_msgs/Quaternion from model coefficients
pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients);
const auto q = obtainCylinderOrientationFromModel(coeffs);
geometry_msgs::Quaternion cyl_q = tf2::toMsg(q);

I hope this would help you!

Upvotes: 0

Related Questions