Я использую библиотеку PCL для получения ориентации обнаруженных объектов. В основном мне нужно только получить OBB объекта (коробка на земле). Для этого я использовал момент инерции из этого урока PCL Tutorial . Итак, сначала я фильтрую облако с помощью фильтра «Сквозной фильтр», а затем выполняю плоскую сегментацию, чтобы удалить первый этаж. И в конце я использовал извлеченное Облако точек (без плоской поверхности), чтобы получить OBB объекта коробки. В конце я визуализирую его как OBB в Rviz (ROS). Здесь код на 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 ();
}
Здесь сцена облака точек.
Затем извлеченный блок и визуализированный 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
Как можно видеть изменение ориентации. Пожалуйста, помогите как улучшить результаты? Спасибо