Visualisation marker not correct orientation
Hi
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>);
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 marker with orientation and position.
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
Asked by Astronaut on 2020-05-04 05:25:42 UTC
Comments
Is this not a duplicate of #q351039? Or at least (very) similar to it?
Asked by gvdhoorn on 2020-05-04 05:56:53 UTC
it is not. That one doesn't have the visualization marker. Anyway, any help?.
Asked by Astronaut on 2020-05-04 09:54:41 UTC
I suspect this is your problem:
You likely want:
Does that give you the expected result?
Asked by tryan on 2020-05-04 15:09:07 UTC
that was a typo.
Asked by Astronaut on 2020-05-05 05:35:40 UTC
ok. I have done some filtering and now the OBB is correct. thanks. Fix it. Now its works
Asked by Astronaut on 2020-05-05 21:50:34 UTC