Visualisation marker not correct orientation

asked 2020-05-04 05:25:42 -0500

Astronaut gravatar image


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 tutorialPCL 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. In the end, I visualize the OBB in Rviz (ROS). Here the code in C++

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>);
    //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.setFilterFieldName ("y");
    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);

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

        // Fit a plane
        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
        //pcl::PointCloud<pcl::PointXYZRGB> cloudF;
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_box (new pcl::PointCloud<pcl::PointXYZRGB> ());

  //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;

  //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 ...
edit retag flag offensive close merge delete


Is this not a duplicate of #q351039? Or at least (very) similar to it?

gvdhoorn gravatar image gvdhoorn  ( 2020-05-04 05:56:53 -0500 )edit

it is not. That one doesn't have the visualization marker. Anyway, any help?.

Astronaut gravatar image Astronaut  ( 2020-05-04 09:54:41 -0500 )edit

I suspect this is your problem:

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();

You likely want:

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();

Does that give you the expected result?

tryan gravatar image tryan  ( 2020-05-04 15:09:07 -0500 )edit

that was a typo.

Astronaut gravatar image Astronaut  ( 2020-05-05 05:35:40 -0500 )edit

ok. I have done some filtering and now the OBB is correct. thanks. Fix it. Now its works

Astronaut gravatar image Astronaut  ( 2020-05-05 21:50:34 -0500 )edit