bounding_volume_calculator.cpp
Go to the documentation of this file.
2 
3 rail_manipulation_msgs::BoundingVolume BoundingVolumeCalculator::computeBoundingVolume(sensor_msgs::PointCloud2 cloud)
4 {
5  //convert point cloud to pcl point cloud
6  pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZ>);
7  pcl::PCLPointCloud2 converter;
8  pcl_conversions::toPCL(cloud, converter);
9  pcl::fromPCLPointCloud2(converter, *object_cloud);
10 
12 }
13 
14 rail_manipulation_msgs::BoundingVolume
15 BoundingVolumeCalculator::computeBoundingVolume(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
16 {
17  pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud(new pcl::PointCloud<pcl::PointXYZ>);
18  pcl::copyPointCloud(*cloud, *xyz_cloud);
19 
21 }
22 
23 rail_manipulation_msgs::BoundingVolume
24 BoundingVolumeCalculator::computeBoundingVolume(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
25 {
26  //calculate original point cloud bounds
27  pcl::PointXYZ min_original, max_original;
28  pcl::getMinMax3D(*cloud, min_original, max_original);
29 
30  // project point cloud to x-y plane (this will create a bounding box that aligns with gravity; not always accurate,
31  // but a useful assumption for tabletop, shelf, and floor pick-and-place applications)
32  pcl::PointCloud<pcl::PointXYZ>::Ptr projected_cloud(new pcl::PointCloud<pcl::PointXYZ>);
33  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients());
34  coefficients->values.resize(4);
35  coefficients->values[0] = coefficients->values[1] = 0;
36  coefficients->values[2] = 1;
37  coefficients->values[3] = 0;
38  pcl::ProjectInliers<pcl::PointXYZ> projector;
39  projector.setModelType(pcl::SACMODEL_PLANE);
40  projector.setInputCloud(cloud);
41  projector.setModelCoefficients(coefficients);
42  projector.filter(*projected_cloud);
43 
44  // compute principal direction
45  Eigen::Matrix3f covariance;
46  Eigen::Vector4f centroid;
47  //use center instead of centroid so as not to strongly weight parts of the point cloud closer to the sensor
48  centroid[0] = static_cast<float>((min_original.x + max_original.x)/2.0);
49  centroid[1] = static_cast<float>((min_original.y + max_original.y)/2.0);
50  centroid[2] = 0; //because point cloud is projected to x-y plane
51  pcl::computeCovarianceMatrixNormalized(*projected_cloud, centroid, covariance);
52  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);
53  Eigen::Matrix3f eig_dx = eigen_solver.eigenvectors();
54  eig_dx.col(2) = eig_dx.col(0).cross(eig_dx.col(1));
55 
56  //move the points to that reference frame
57  Eigen::Matrix4f p2w(Eigen::Matrix4f::Identity());
58  p2w.block(0, 0, 3, 3) = eig_dx.transpose();
59  p2w.block(0, 3, 3, 1) = -1.f * (p2w.block(0, 0, 3, 3) * centroid.head(3));
60  pcl::PointCloud<pcl::PointXYZ> c_points;
61  pcl::transformPointCloud(*projected_cloud, c_points, p2w);
62 
63  pcl::PointXYZ min_pt, max_pt;
64  pcl::getMinMax3D(c_points, min_pt, max_pt);
65  const Eigen::Vector3f mean_diag = 0.5f * (max_pt.getVector3fMap() + min_pt.getVector3fMap());
66 
67  //final transform
68  const Eigen::Quaternionf qfinal(eig_dx);
69  const Eigen::Vector3f tfinal = eig_dx * mean_diag + centroid.head(3);
70 
71  //set object shape
72  rail_manipulation_msgs::BoundingVolume bounding_box;
73  bounding_box.dimensions.x = max_original.z - min_original.z;
74  bounding_box.dimensions.y = max_pt.y - min_pt.y;
75  bounding_box.dimensions.z = max_pt.z - min_pt.z;
76  bounding_box.pose.header.frame_id = cloud->header.frame_id;
77  bounding_box.pose.pose.position.x = tfinal[0];
78  bounding_box.pose.pose.position.y = tfinal[1];
79  bounding_box.pose.pose.position.z = (min_original.z + max_original.z)/2.0;
80  bounding_box.pose.pose.orientation.w = qfinal.w();
81  bounding_box.pose.pose.orientation.x = qfinal.x();
82  bounding_box.pose.pose.orientation.y = qfinal.y();
83  bounding_box.pose.pose.orientation.z = qfinal.z();
84 
85  return bounding_box;
86 }
static rail_manipulation_msgs::BoundingVolume computeBoundingVolume(sensor_msgs::PointCloud2 cloud)
Fit a z-axis-aligned bounding box to x-y plane principal direction of point cloud.
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)


rail_segmentation
Author(s): Russell Toris , David Kent
autogenerated on Sun Jun 30 2019 19:58:20