Not correct orientation of the OBB Box

Question

Im using PCL library to obtain the orientation of detected objects. Basically i only need to get the OBB of the object (Box on the ground). So for that I was using the Moment of Inertia from this tutorial PCL Tutorial. So first, I filter the cloud using the Pass through Filter, then done the planar segmentation to remove the ground floor. And at the end I used the extracted point cloud Box( without the plane surface) to get the OBB of the box object. At the end I visualize it the OBB in Rviz (ROS). Here the code in C++ (PCL and ROS).

ros::Publisher pub, markers_pub_;

void cloud_cb(const sensor_msgs::PointCloud2::ConstPtr &msg){

    // Convert to pcl point cloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_msg (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg(*msg,*cloud_msg);
    //ROS_DEBUG("%s: new ponitcloud (%i,%i)(%zu)",_name.c_str(),cloud_msg->width,cloud_msg->height,cloud_msg->size());

    // Filter cloud
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud(cloud_msg);
    pass.setFilterFieldName ("y");
    pass.setFilterLimits(0.001,10);
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
    pass.filter (*cloud);

    // Get segmentation ready
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.04);

    // Create pointcloud to publish inliers
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_pub(new pcl::PointCloud<pcl::PointXYZRGB>);
    

        // Fit a plane
        seg.setInputCloud(cloud);
        seg.segment(*inliers, *coefficients);

        // Check result
        if (inliers->indices.size() == 0)
           {
            ROS_WARN_STREAM ("Could not estimate a planar model for the given dataset.") ;
            }


        // Extract inliers
        extract.setInputCloud(cloud);
        extract.setIndices(inliers);
        extract.setNegative(true);
        //pcl::PointCloud<pcl::PointXYZRGB> cloudF;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_box (new pcl::PointCloud<pcl::PointXYZRGB> ());
        extract.filter(*cloud_box);
       
  //Moment of Inertia
  pcl::MomentOfInertiaEstimation <pcl::PointXYZRGB> feature_extractor;
  feature_extractor.setInputCloud (cloud_box);
  feature_extractor.compute ();

  std::vector <float> moment_of_inertia;
  std::vector <float> eccentricity;

  pcl::PointXYZRGB min_point_OBB;
  pcl::PointXYZRGB max_point_OBB;
  pcl::PointXYZRGB position_OBB;
  Eigen::Matrix3f rotational_matrix_OBB;
  float major_value, middle_value, minor_value;
  Eigen::Vector3f major_vector, middle_vector, minor_vector;
  Eigen::Vector3f mass_center;

  feature_extractor.getMomentOfInertia (moment_of_inertia);
  feature_extractor.getEccentricity (eccentricity);
  feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
  feature_extractor.getEigenValues (major_value, middle_value, minor_value);
  feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
  feature_extractor.getMassCenter (mass_center);

  Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
  Eigen::Quaternionf quat (rotational_matrix_OBB);
   
  cout << " orientation x  = " << quat.x() <<  endl;
  cout << " orientation y = "  << quat.y() <<  endl;
  cout << " orientation z = "  << quat.z() <<  endl;
  cout << " orientation w = "  << quat.w() <<  endl;
  cout << " postion x  = " << position_OBB.x <<  endl;
  cout << " postion y  = " << position_OBB.y <<  endl;
  cout << " postion z  = " << position_OBB.z <<  endl;
   

    // Publish points
    sensor_msgs::PointCloud2 cloud_publish;
    pcl::toROSMsg(*cloud_box,cloud_publish);
    pub.publish(cloud_publish);

  //Visualisation Marker
  std::string ns; 
  float r; 
  float g; 
  float b;
  visualization_msgs::MarkerArray msg_marker;
  visualization_msgs::Marker bbx_marker;
  bbx_marker.header.frame_id = "zed_left_camera_frame";
  bbx_marker.header.stamp = ros::Time::now();
  bbx_marker.ns = ns;
  bbx_marker.type = visualization_msgs::Marker::CUBE;
  bbx_marker.action = visualization_msgs::Marker::ADD;
  bbx_marker.pose.position.x =  position_OBB.x;
  bbx_marker.pose.position.y =  position_OBB.y;
  bbx_marker.pose.position.z =  position_OBB.z;
  bbx_marker.pose.orientation.x = quat.x();
  bbx_marker.pose.orientation.y = quat.x();
  bbx_marker.pose.orientation.z = quat.x();
  bbx_marker.pose.orientation.w = quat.x();
  bbx_marker.scale.x = (max_point_OBB.x - min_point_OBB.x);
  bbx_marker.scale.y = (max_point_OBB.y - min_point_OBB.y);
  bbx_marker.scale.z = (max_point_OBB.z - min_point_OBB.z);
  bbx_marker.color.b = b;
  bbx_marker.color.g = g;
  bbx_marker.color.r = r;
  bbx_marker.color.a = 0.4;
  bbx_marker.lifetime = ros::Duration();
  msg_marker.markers.push_back(bbx_marker);
  markers_pub_.publish(msg_marker);

}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;
  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("/zed/zed_node/point_cloud/cloud_registered", 200, cloud_cb);
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<sensor_msgs::PointCloud2> ("cloud_publish", 100);
  markers_pub_ = nh.advertise<visualization_msgs::MarkerArray> ("msg_marker", 100);
    ros::spin();
  ros::spin ();
}

Here the point cloud scene.

Then the extracted box and the visualized OBB.

So my question is why the orientation of the OBB is not correct? is not aligned with the red box. Also the output is following

orientation x  = 0.553429
 orientation y = 0.409076
 orientation z = 0.575402
 orientation w = 0.441912
 postion x  = 0.688811
 postion y  = 0.296049
 postion z  = -0.0444195
 orientation x  = 0.551899
 orientation y = 0.41675
 orientation z = 0.556839
 orientation w = 0.460061
 postion x  = 0.6858
 postion y  = 0.297214
 postion z  = -0.0479018
 orientation x  = -0.447575
 orientation y = 0.523119
 orientation z = -0.488997
 orientation w = 0.535635
 postion x  = 0.687003
 postion y  = 0.296398
 postion z  = -0.0541157
 orientation x  = -0.435059
 orientation y = 0.533038
 orientation z = -0.483508
 orientation w = 0.541123
 postion x  = 0.689015
 postion y  = 0.299274
 postion z  = -0.0532807
 orientation x  = -0.483639
 orientation y = 0.486945
 orientation z = -0.526589
 orientation w = 0.50168
 postion x  = 0.687567
 postion y  = 0.290984
 postion z  = -0.0566443
 orientation x  = -0.451907
 orientation y = 0.514618
 orientation z = -0.499482
 orientation w = 0.530533
 postion x  = 0.688489
 postion y  = 0.300407
 postion z  = -0.0544657
 orientation x  = -0.462979
 orientation y = 0.508457
 orientation z = -0.503387
 orientation w = 0.523185
 postion x  = 0.687322
 postion y  = 0.294014
 postion z  = -0.0556483
 orientation x  = 0.507688
 orientation y = 0.462501
 orientation z = 0.530055
 orientation w = 0.497381
 postion x  = 0.687552
 postion y  = 0.293263
 postion z  = -0.055368
 orientation x  = -0.413774
 orientation y = 0.554115
 orientation z = -0.456901
 orientation w = 0.559455

As can see orientation change as well. Please, any help how to improve the results? Thanks

Answer 1

Problem solved. It was a typo in

bbx_marker.pose.orientation.x = quat.x();
bbx_marker.pose.orientation.y = quat.x();
bbx_marker.pose.orientation.z = quat.x();
bbx_marker.pose.orientation.w = quat.x();

I should be

bbx_marker.pose.orientation.x = quat.x();
  bbx_marker.pose.orientation.y = quat.y();
  bbx_marker.pose.orientation.z = quat.z();
  bbx_marker.pose.orientation.w = quat.w();

Answer 2

Usually when there's an issue with rotations in 3D, the frames of reference used by different libraries is often different and need correction before being passes between them (in this case ROS and PCL). I can't say if that's the problem here.

However, I also see the following issue in the code presented:

  // notice all assignments are from x() to {x,y,z,w}
  // they should be from x() to x, y() to y and so on and so forth
  bbx_marker.pose.orientation.x = quat.x();
  bbx_marker.pose.orientation.y = quat.x();
  bbx_marker.pose.orientation.z = quat.x();
  bbx_marker.pose.orientation.w = quat.x();

It appears that either

1. you're not assigning the orientation correctly OR
2. you've not synced the code and images with your observations