OBB盒的方向不正确

Question

Im使用PCL库获取检测到的对象的方向。基本上我只需要获取对象（地面上的框）的OBB。因此，我正在使用本教程PCL Tutorial中的惯性矩。因此，首先，我使用“通过滤镜”对云进行滤镜，然后进行平面分割以去除底层。最后，我使用提取的点云Box（没有平面）来获取box对象的OBB。最后，我将其可视化为Rviz（ROS）中的OBB。这里是C ++中的代码（PCL和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 ();
}

这里是点云场景。

<< img src =“ https://image.soinside.com/eyJ1cmwiOiAiaHR0cHM6Ly9pLnN0YWNrLmltZ3VyLmNvbS9pTEhFUy5wbmcifQ==” alt =“整个点云场景”>

然后是提取的框和可视化的OBB。

所以我的问题是，为什么OBB的方向不正确？与红色框不对齐。输出也如下

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

也可以看到方向改变。请帮忙如何改善效果？谢谢

Answer 1

通常，当3D旋转出现问题时，不同库使用的参照系通常会有所不同，并且需要在它们之间传递之前进行校正（在本例中为ROS和PCL）。我不能说这是这里的问题。

但是，我在所提供的代码中也看到以下问题：

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

似乎出现

1. 您没有正确分配方向OR
2. 您尚未将代码和图像与观察结果同步

Answer 2

问题已解决。

是个错字

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