PCL自带OBB包围盒和自写的平面包围盒，都已转到平面

/*PCL自带的obb库函数*/
pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
feature_extractor.setInputCloud (scene_trans);
feature_extractor.compute ();
std::vector <float> moment_of_inertia;
std::vector <float> eccentricity;
pcl::PointXYZ min_point_OBB;
pcl::PointXYZ max_point_OBB;
pcl::PointXYZ position_OBB;
Eigen::Matrix3f rotational_matrix_OBB;
float major_value, middle_value, minor_value;
Eigen::Vector3f major_vector, middle_vector, minor_vector;
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);
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
viewer->addPointCloud<pcl::PointXYZ> (scene_trans, "scene cloud");
std::cout <<"点云的曲面法线是(" << minor_vector[0] << "," << minor_vector[1] << "," << minor_vector[2] << ")." <<std::endl;
Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
Eigen::Quaternionf quat (rotational_matrix_OBB);
viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "OBB");
while(!viewer->wasStopped())
{
// viewer->spinOnce (100);
// boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}

 

 

 

float divide_scale = 0.1;
double area, min_area = 100000000;
pcl::PointXYZ min_last, max_last;
pcl::PointXYZ min, max;
pcl::PointCloud<pcl::PointXYZ>::Ptr scene_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scene_out(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr scene_all_out(new pcl::PointCloud<pcl::PointXYZ>);
int scale_num = 90 / divide_scale;
Eigen::Matrix4f z_rot_transform = Eigen::Matrix4f::Identity();
for (int i = 0; i < scale_num; i++)
{
Eigen::Matrix4f z_rot_mat = Eigen::Matrix4f::Identity();
Eigen::AngleAxisf z_rot_angle_axis(i * divide_scale * M_PI / 180.0, Eigen::Vector3f::UnitZ());
Eigen::Matrix3f z_rot_rot_angle_axis = z_rot_angle_axis.toRotationMatrix();
z_rot_mat.block<3, 3>(0, 0) = z_rot_rot_angle_axis;
pcl::transformPointCloud(*scene_trans, *scene_in, z_rot_mat);
pcl::getMinMax3D(*scene_in, min, max);
area = abs((max.x - min.x) * (max.y - min.y));
// std::cout<<"area:"<<area<<endl;
if (min_area > area)
{
min_area = area;
min_last = min;
max_last = max;
z_rot_transform = z_rot_mat;
}
}
pcl::transformPointCloud(*scene_trans, *scene_all_out, z_rot_transform);
// std::cout<<"10"<<endl;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
viewer->setBackgroundColor(0, 0, 0);
viewer->addPointCloud<pcl::PointXYZ>(scene_all_out, "scene cloud");
// Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
// Eigen::Quaternionf quat (rotational_matrix_OBB);
// viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");
viewer->addCube(min_last.x, max_last.x, min_last.y, max_last.y, min_last.z, max_last.z, 1, 0, 0, "cube");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "cube");
viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "cube");
while (!viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep(boost::posix_time::microseconds(100000));
}