20201011-pcl构造AABB\OBB包围盒

20201011-pcl构造AABB\OBB包围盒

//TODO::头文件定义
#include<pcl/point_cloud.h>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<vector>
#include<vtkAutoInit.h>
#include<pcl/visualization/cloud_viewer.h>
#include<pcl/features/moment_of_inertia_estimation.h>

//TODO::主函数
int main(int argc, char** argv)
{
	//TODO::为点云实例化-cloud
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	//TODO::载入点云数据集
	if (pcl::io::loadPCDFile("bun0.pcd", *cloud) == -1)
	{
		PCL_ERROR("cloudn`t read file");
		system("pause");//暂留黑窗口
		return -1;
	}
	//TODO::定义一些需要用到的变量向量
	//TODO::惯性矩moment_of_inertia
	std::vector <float> moment_of_inertia;
	//TODO::偏心率eccentricity
	std::vector <float> eccentricity;
	//TODO::声明AABB的最小三坐标min_point_AABB和最大三坐标max_point_AABB
	pcl::PointXYZ min_point_AABB;
	pcl::PointXYZ max_point_AABB;
	//TODO::声明OBB的最小三坐标min_point_OBB和最大三坐标max_point_OBB
	pcl::PointXYZ min_point_OBB;
	pcl::PointXYZ max_point_OBB;
	//TODO::声明OBB position_OBB
	pcl::PointXYZ position_OBB;
	//TODO::声明旋转矩阵rotational_matrix_OBB
	Eigen::Matrix3f rotational_matrix_OBB;
	//TODO::声明特征向量major_vector, middle_vector, minor_vector与特征值major_value, middle_value, minor_value
	float major_value, middle_value, minor_value;
	Eigen::Vector3f major_vector, middle_vector, minor_vector;
	//TODO::声明质心向量mass_center
	Eigen::Vector3f mass_center;

	//TODO::实例化惯性矩特征提取feature_extractor
	pcl::MomentOfInertiaEstimation<pcl::PointXYZ> feature_extractor;
	feature_extractor.setInputCloud(cloud);//输入待求包围盒的点云数据集
	feature_extractor.compute();//计算特征
	//TODO::计算AABB包围盒 1.惯性矩moment_of_inertia; 2.偏心率eccentricity; 3.AABB的六个极值坐标
	feature_extractor.getMomentOfInertia(moment_of_inertia);
	feature_extractor.getEccentricity(eccentricity);
	feature_extractor.getAABB(min_point_AABB, max_point_AABB);
	//TODO::计算OBB包围盒 1.六个极值坐标 2.位置position_OBB 3.旋转矩阵rotational_matrix_OBB 4.特征值特征向量 5.质心mass_center
	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);

	//TODO::可视化 首先一个实例化
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
	viewer->setBackgroundColor(1, 1, 1);
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> green(cloud, 0, 255, 0);
	//TODO::初始化相机参数，设置坐标系以及窗口背景颜色。接下来设置点云颜色为绿色，并向窗口添加点云，即cloud为绿色，在窗口中id为sample cloud
	viewer->addPointCloud<pcl::PointXYZ>(cloud, green, "sample cloud");
	//TODO::设置id=sample cloud的点云的点的大小
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "sample cloud");
	//TODO::添加AABB cube,设置为红色，框的id=AABB
	viewer->addCube(min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z,1.0,0.0,0.0,"AABB");
	//TODO::设置opacity和线宽
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.1, "AABB");
	viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 4, "AABB");

	Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
	std::cout << "position_OBB" << position_OBB << std::endl;
	std::cout << "mass_center" << mass_center << std::endl;

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
	}
	system("pause");
	return (0);
}```